From 04a62b813ffd8e143554141d3394107f8a016957 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 28 Jun 2019 09:32:46 +0200 Subject: [PATCH 01/64] Remove unused CMake modules Move ENABLE_GNSS_SIM_INSTALL option to main CMakeLisis.txt file Remove unused lines Update Doxygen download website --- CMakeLists.txt | 10 +- cmake/Modules/FindLIBOSMOSDR.cmake | 98 ------------------- cmake/Modules/FindOPENBLAS.cmake | 49 ---------- cmake/Modules/TestForSSE.cmake | 55 ----------- .../signal_source/adapters/CMakeLists.txt | 2 - src/tests/CMakeLists.txt | 4 +- 6 files changed, 10 insertions(+), 208 deletions(-) delete mode 100644 cmake/Modules/FindLIBOSMOSDR.cmake delete mode 100644 cmake/Modules/FindOPENBLAS.cmake delete mode 100644 cmake/Modules/TestForSSE.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 05207272c..0b3d443cc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -97,9 +97,16 @@ option(ENABLE_SYSTEM_TESTING "Build system tests" OFF) option(ENABLE_SYSTEM_TESTING_EXTRA "Download external tools and build extra system tests" OFF) +option(ENABLE_GNSS_SIM_INSTALL "Enable the installation of gnss_sim on the fly" ON) + +if(NOT (ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)) + set(ENABLE_GNSS_SIM_INSTALL OFF) +endif() + if(ENABLE_SYSTEM_TESTING_EXTRA) set(ENABLE_SYSTEM_TESTING ON) endif() + option(ENABLE_OWN_GPSTK "Force to download, build and link GPSTk for system tests, even if it is already installed" OFF) option(ENABLE_INSTALL_TESTS "Install QA code system-wide" OFF) @@ -2026,7 +2033,7 @@ if(DOXYGEN_FOUND) else() message(STATUS " Doxygen has not been found in your system.") message(STATUS " You can get nice code documentation by using it!") - message(STATUS " Get it from http://www.stack.nl/~dimitri/doxygen/index.html") + message(STATUS " Get it from http://www.doxygen.nl/download.html") if(OS_IS_LINUX) if(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") message(STATUS " or simply by doing 'sudo yum install doxygen-latex'.") @@ -2466,6 +2473,7 @@ add_feature_info(ENABLE_UNIT_TESTING_EXTRA ENABLE_UNIT_TESTING_EXTRA "Enables bu add_feature_info(ENABLE_SYSTEM_TESTING ENABLE_SYSTEM_TESTING "Enables building of System Tests.") add_feature_info(ENABLE_SYSTEM_TESTING_EXTRA ENABLE_SYSTEM_TESTING_EXTRA "Enables building of Extra System Tests and downloading of external tools.") add_feature_info(ENABLE_OWN_GPSTK ENABLE_OWN_GPSTK "Forces the downloading and building of GPSTk for system tests.") +add_feature_info(ENABLE_GNSS_SIM_INSTALL ENABLE_GNSS_SIM_INSTALL "Enables downloading and building of gnss-sim.") add_feature_info(ENABLE_INSTALL_TESTS ENABLE_INSTALL_TESTS "Install test binaries when doing '${CMAKE_MAKE_PROGRAM_PRETTY_NAME} install'.") message(STATUS "") diff --git a/cmake/Modules/FindLIBOSMOSDR.cmake b/cmake/Modules/FindLIBOSMOSDR.cmake deleted file mode 100644 index 7ecf41a61..000000000 --- a/cmake/Modules/FindLIBOSMOSDR.cmake +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright (C) 2011-2018 (see AUTHORS file for a list of contributors) -# -# This file is part of GNSS-SDR. -# -# GNSS-SDR is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# GNSS-SDR is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with GNSS-SDR. If not, see . - -# Tries to find libosmosdr. -# -# Usage of this module as follows: -# -# find_package(LIBOSMOSDR) -# -# -# Variables defined by this module: -# -# LIBOSMOSDR_FOUND System has libosmosdr libs/headers -# LIBOSMOSDR_LIBRARIES The libosmosdr libraries -# LIBOSMOSDR_INCLUDE_DIR The location of libosmosdr headers -# -# Provides the following imported target: -# Osmosdr::osmosdr -# - -set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE) -include(FindPkgConfig) -pkg_check_modules(LIBOSMOSDR_PKG libosmosdr) - -find_path(LIBOSMOSDR_INCLUDE_DIR NAMES osmosdr.h - PATHS - ${LIBOSMOSDR_PKG_INCLUDE_DIRS} - /usr/include - /usr/local/include - ${LIBOSMOSDR_ROOT}/include - $ENV{LIBOSMOSDR_ROOT}/include - ${LIBOSMOSDR_PKG_INCLUDEDIR} -) - -find_library(LIBOSMOSDR_LIBRARIES NAMES osmosdr - PATHS - ${LIBOSMOSDR_PKG_LIBRARY_DIRS} - /usr/lib - /usr/local/lib - /usr/lib/x86_64-linux-gnu - /usr/lib/i386-linux-gnu - /usr/lib/arm-linux-gnueabihf - /usr/lib/arm-linux-gnueabi - /usr/lib/aarch64-linux-gnu - /usr/lib/mipsel-linux-gnu - /usr/lib/mips-linux-gnu - /usr/lib/mips64el-linux-gnuabi64 - /usr/lib/powerpc-linux-gnu - /usr/lib/powerpc64-linux-gnu - /usr/lib/powerpc64le-linux-gnu - /usr/lib/powerpc-linux-gnuspe - /usr/lib/hppa-linux-gnu - /usr/lib/s390x-linux-gnu - /usr/lib/i386-gnu - /usr/lib/hppa-linux-gnu - /usr/lib/x86_64-kfreebsd-gnu - /usr/lib/i386-kfreebsd-gnu - /usr/lib/m68k-linux-gnu - /usr/lib/sh4-linux-gnu - /usr/lib/sparc64-linux-gnu - /usr/lib/x86_64-linux-gnux32 - /usr/lib/alpha-linux-gnu - /usr/lib64 - ${LIBOSMOSDR_ROOT}/lib - $ENV{LIBOSMOSDR_ROOT}/lib - ${LIBOSMOSDR_ROOT}/lib64 - $ENV{LIBOSMOSDR_ROOT}/lib64 - ${LIBOSMOSDR_PKG_LIBDIR} -) - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(LIBOSMOSDR DEFAULT_MSG LIBOSMOSDR_INCLUDE_DIR LIBOSMOSDR_LIBRARIES) - -if(LIBOSMOSDR_FOUND AND NOT TARGET Osmosdr::osmosdr) - add_library(Osmosdr::osmosdr SHARED IMPORTED) - set_target_properties(Osmosdr::osmosdr PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${LIBOSMOSDR_LIBRARIES}" - INTERFACE_INCLUDE_DIRECTORIES "${LIBOSMOSDR_INCLUDE_DIR}" - INTERFACE_LINK_LIBRARIES "${LIBOSMOSDR_LIBRARIES}" - ) -endif() - -mark_as_advanced(LIBOSMOSDR_INCLUDE_DIR LIBOSMOSDR_LIBRARIES) diff --git a/cmake/Modules/FindOPENBLAS.cmake b/cmake/Modules/FindOPENBLAS.cmake deleted file mode 100644 index 8ff70d65f..000000000 --- a/cmake/Modules/FindOPENBLAS.cmake +++ /dev/null @@ -1,49 +0,0 @@ -# Copyright (C) 2011-2018 (see AUTHORS file for a list of contributors) -# -# This file is part of GNSS-SDR. -# -# GNSS-SDR is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# GNSS-SDR is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with GNSS-SDR. If not, see . - -# - Try to find OpenBLAS library (not headers!) -# -# The following environment variable is optionally searched -# OPENBLAS_HOME: Base directory where all OpenBlas components are found - -set(OPEN_BLAS_SEARCH_PATHS - /lib - /lib64/ - /usr/lib - /usr/lib64 - /usr/local/lib - /usr/local/lib64 - /opt/OpenBLAS/lib - /opt/local/lib - /usr/lib/openblas-base - $ENV{OPENBLAS_HOME}/lib - ${OPENBLAS_ROOT}/lib - $ENV{OPENBLAS_ROOT}/lib - ${OPENBLAS_ROOT}/lib64 - $ENV{OPENBLAS_ROOT}/lib64 -) - -find_library(OPENBLAS NAMES openblas PATHS ${OPEN_BLAS_SEARCH_PATHS}) - -if(OPENBLAS) - set(OPENBLAS_FOUND ON) - message(STATUS "Found OpenBLAS") -endif() - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(OPENBLAS DEFAULT_MSG OPENBLAS) -mark_as_advanced(OPENBLAS) diff --git a/cmake/Modules/TestForSSE.cmake b/cmake/Modules/TestForSSE.cmake deleted file mode 100644 index 23b2d16a8..000000000 --- a/cmake/Modules/TestForSSE.cmake +++ /dev/null @@ -1,55 +0,0 @@ -# Copyright (C) 2011-2018 (see AUTHORS file for a list of contributors) -# -# This file is part of GNSS-SDR. -# -# GNSS-SDR is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# GNSS-SDR is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with GNSS-SDR. If not, see . - - -############################################################################### -# Test for availability of SSE -# -# - Anthony Arnold -############################################################################### - -function(test_for_sse h_file result_var name) - if(NOT DEFINED ${result_var}) - execute_process(COMMAND echo "#include <${h_file}>" - COMMAND ${CMAKE_CXX_COMPILER} ${CMAKE_CXX_COMPILER_ARG1} -c -x c++ - - RESULT_VARIABLE COMPILE_RESULT - OUTPUT_QUIET ERROR_QUIET) - set(detected 0) - if(COMPILE_RESULT EQUAL 0) - message(STATUS "Detected ${name}") - set(detected 1) - endif() - set(${result_var} ${detected} CACHE INTERNAL "${name} Available") - endif() -endfunction() - -message(STATUS "Testing for SIMD extensions") - -enable_language(C) - -test_for_sse("ammintrin.h" SSE4A_AVAILABLE "SSE4A") -test_for_sse("nmmintrin.h" SSE4_2_AVAILABLE "SSE4.2") -test_for_sse("smmintrin.h" SSE4_1_AVAILABLE "SSE4.1") -test_for_sse("tmmintrin.h" SSSE3_AVAILABLE "SSSE3") -test_for_sse("pmmintrin.h" SSE3_AVAILABLE "SSE3") -test_for_sse("emmintrin.h" SSE2_AVAILABLE "SSE2") -test_for_sse("xmmintrin.h" SSE_AVAILABLE "SSE1") -test_for_sse("mmintrin.h" MMX_AVAILABLE "MMX") -test_for_sse("wmmintrin.h" AES_AVAILABLE "AES") -test_for_sse("immintrin.h" AVX_AVAILABLE "AVX") - -file(REMOVE "${CMAKE_CURRENT_BINARY_DIR}/-.o") diff --git a/src/algorithms/signal_source/adapters/CMakeLists.txt b/src/algorithms/signal_source/adapters/CMakeLists.txt index e9cba3c93..1cb23518b 100644 --- a/src/algorithms/signal_source/adapters/CMakeLists.txt +++ b/src/algorithms/signal_source/adapters/CMakeLists.txt @@ -103,8 +103,6 @@ if(ENABLE_ARRAY) message(" gr-dbfcttc not found, install it from https://github.com/gnss-sdr/gr-dbfcttc ") message(FATAL_ERROR "gr-dbfcttc required for building gnss-sdr with this option enabled") endif() - set(OPT_LIBRARIES ${OPT_LIBRARIES} ${GR_DBFCTTC_LIBRARIES}) - set(OPT_DRIVER_INCLUDE_DIRS ${OPT_DRIVER_INCLUDE_DIRS} ${GR_DBFCTTC_INCLUDE_DIRS}) set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} raw_array_signal_source.cc) set(OPT_DRIVER_HEADERS ${OPT_DRIVER_HEADERS} raw_array_signal_source.h) endif() diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index e3bde0ee9..5259370d2 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -191,10 +191,8 @@ endif() ################################################################################ -# Optional generator +# GPSTk and optional generator ################################################################################ -option(ENABLE_GNSS_SIM_INSTALL "Enable the installation of gnss_sim on the fly" ON) -add_feature_info(ENABLE_GNSS_SIM_INSTALL ENABLE_GNSS_SIM_INSTALL "Enables downloading and building of gnss-sim.") find_package(GPSTK) set_package_properties(GPSTK PROPERTIES From 1cd7ca301d1d41cc00c1c258454a1969bbd2eb2d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 28 Jun 2019 12:26:09 +0200 Subject: [PATCH 02/64] Use std::array for compile-time bound checking --- src/algorithms/PVT/libs/nmea_printer.cc | 4 ++-- src/algorithms/PVT/libs/rtklib_solver.cc | 19 ++++++++----------- src/algorithms/PVT/libs/rtklib_solver.h | 4 ++-- 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index c70deb403..ecc92b914 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -441,7 +441,7 @@ std::string Nmea_Printer::get_GPGSA() // GSA-GNSS DOP and Active Satellites std::stringstream sentence_str; unsigned char buff[1024] = {0}; - outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat); + outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat.data()); sentence_str << buff; return sentence_str.str(); } @@ -454,7 +454,7 @@ std::string Nmea_Printer::get_GPGSV() // Notice that NMEA 2.1 only supports 12 channels std::stringstream sentence_str; unsigned char buff[1024] = {0}; - outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat); + outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat.data()); sentence_str << buff; return sentence_str.str(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index ecf0efe9a..79d52b467 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -507,9 +507,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ int valid_obs = 0; // valid observations counter int glo_valid_obs = 0; // GLONASS L1/L2 valid observations counter - obsd_t obs_data[MAXOBS]; - eph_t eph_data[MAXOBS]; - geph_t geph_data[MAXOBS]; + std::array obs_data; + std::array eph_data; + std::array geph_data; // Workaround for NAV/CNAV clash problem bool gps_dual_band = false; @@ -894,8 +894,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ int result = 0; int sat = 0; nav_t nav_data; - nav_data.eph = eph_data; - nav_data.geph = geph_data; + nav_data.eph = eph_data.data(); + nav_data.geph = geph_data.data(); nav_data.n = valid_obs; nav_data.ng = glo_valid_obs; @@ -915,7 +915,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } } - result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data); + result = rtkpos(&rtk_, obs_data.data(), valid_obs + glo_valid_obs, &nav_data); if (result == 0) { @@ -976,7 +976,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration - //compute Ground speed and COG + // compute Ground speed and COG double ground_speed_ms = 0.0; double pos[3]; double enuv[3]; @@ -1015,9 +1015,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ << " [deg], Height= " << this->get_height() << " [m]" << " RX time offset= " << this->get_time_offset_s() << " [s]"; - - // PVT MONITOR - + // ######## PVT MONITOR ######### // TOW monitor_pvt.TOW_at_current_symbol_ms = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms; // WEEK @@ -1067,7 +1065,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ monitor_pvt.hdop = dop_[2]; monitor_pvt.vdop = dop_[3]; - // ######## LOG FILE ######### if (d_flag_dump_enabled == true) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 6457d28d6..b13e2446f 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -84,7 +84,7 @@ /*! - * \brief This class implements a simple PVT Least Squares solution + * \brief This class implements a PVT solution based on RTKLIB */ class Rtklib_Solver : public Pvt_Solution { @@ -107,7 +107,7 @@ public: bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); sol_t pvt_sol; - ssat_t pvt_ssat[MAXSAT]; + std::array pvt_ssat; double get_hdop() const; double get_vdop() const; double get_pdop() const; From a2c6c8a630966f3fe1c17180e3021b79045019cf Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 28 Jun 2019 12:26:46 +0200 Subject: [PATCH 03/64] Apply clang-tidy fixes --- src/algorithms/libs/opencl/cl.hpp | 38 +++++++++++++++---------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/algorithms/libs/opencl/cl.hpp b/src/algorithms/libs/opencl/cl.hpp index b35f6e9cf..39e96f46b 100644 --- a/src/algorithms/libs/opencl/cl.hpp +++ b/src/algorithms/libs/opencl/cl.hpp @@ -1203,7 +1203,7 @@ inline cl_int getInfoHelper(Func f, cl_uint name, VECTOR_CLASS* param, long) * template will provide a better match. */ template -inline cl_int getInfoHelper(Func f, cl_uint name, VECTOR_CLASS* param, int, typename T::cl_type = 0) +inline cl_int getInfoHelper(Func f, cl_uint name, VECTOR_CLASS* param, int, typename T::cl_type = nullptr) { ::size_t required; cl_int err = f(name, 0, nullptr, &required); @@ -1330,7 +1330,7 @@ struct ReferenceHandler; * template will provide a better match. */ template -inline cl_int getInfoHelper(Func f, cl_uint name, T* param, int, typename T::cl_type = 0) +inline cl_int getInfoHelper(Func f, cl_uint name, T* param, int, typename T::cl_type = nullptr) { typename T::cl_type value; cl_int err = f(name, sizeof(value), &value, nullptr); @@ -3447,7 +3447,7 @@ public: } else { - object_ = ::clCreateBuffer(context(), flags, size, 0, &error); + object_ = ::clCreateBuffer(context(), flags, size, nullptr, &error); } detail::errHandler(error, __CREATE_BUFFER_ERR); @@ -3881,7 +3881,7 @@ public: { CL_MEM_OBJECT_IMAGE1D, width, - 0, 0, 0, 0, 0, 0, 0, 0}; + 0, 0, 0, 0, 0, 0, 0, nullptr}; object_ = ::clCreateImage( context(), flags, @@ -4048,7 +4048,7 @@ public: 0, 0, // height, depth (unused) arraySize, rowPitch, - 0, 0, 0, 0}; + 0, 0, 0, nullptr}; object_ = ::clCreateImage( context(), flags, @@ -4155,7 +4155,7 @@ public: height, 0, 0, // depth, array size (unused) row_pitch, - 0, 0, 0, 0}; + 0, 0, 0, nullptr}; object_ = ::clCreateImage( context(), flags, @@ -4359,7 +4359,7 @@ public: arraySize, rowPitch, slicePitch, - 0, 0, 0}; + 0, 0, nullptr}; object_ = ::clCreateImage( context(), flags, @@ -4469,7 +4469,7 @@ public: 0, // array size (unused) row_pitch, slice_pitch, - 0, 0, 0}; + 0, 0, nullptr}; object_ = ::clCreateImage( context(), flags, @@ -5722,11 +5722,11 @@ inline VECTOR_CLASS cl::Program::getInfo(cl_int* err { VECTOR_CLASS< ::size_t> sizes = getInfo(); VECTOR_CLASS binaries; - for (VECTOR_CLASS< ::size_t>::iterator s = sizes.begin(); s != sizes.end(); ++s) + for (unsigned long & size : sizes) { char* ptr = nullptr; - if (*s != 0) - ptr = new char[*s]; + if (size != 0) + ptr = new char[size]; binaries.push_back(ptr); } @@ -6535,8 +6535,8 @@ public: * have completed. */ cl_int enqueueMarkerWithWaitList( - const VECTOR_CLASS* events = 0, - Event* event = 0) + const VECTOR_CLASS* events = nullptr, + Event* event = nullptr) { cl_event tmp; cl_int err = detail::errHandler( @@ -6565,8 +6565,8 @@ public: * before this command to command_queue, have completed. */ cl_int enqueueBarrierWithWaitList( - const VECTOR_CLASS* events = 0, - Event* event = 0) + const VECTOR_CLASS* events = nullptr, + Event* event = nullptr) { cl_event tmp; cl_int err = detail::errHandler( @@ -6933,7 +6933,7 @@ Buffer::Buffer( } else { - object_ = ::clCreateBuffer(context(), flags, size, 0, &error); + object_ = ::clCreateBuffer(context(), flags, size, nullptr, &error); } detail::errHandler(error, __CREATE_BUFFER_ERR); @@ -6996,7 +6996,7 @@ Buffer::Buffer( } else { - object_ = ::clCreateBuffer(context(), flags, size, 0, &error); + object_ = ::clCreateBuffer(context(), flags, size, nullptr, &error); } detail::errHandler(error, __CREATE_BUFFER_ERR); @@ -7185,7 +7185,7 @@ inline cl_int copy(const CommandQueue& queue, IteratorType startIterator, Iterat ::size_t byteLength = length * sizeof(DataType); DataType* pointer = - static_cast(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_WRITE, 0, byteLength, 0, 0, &error)); + static_cast(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_WRITE, 0, byteLength, nullptr, nullptr, &error)); // if exceptions enabled, enqueueMapBuffer will throw if (error != CL_SUCCESS) { @@ -7226,7 +7226,7 @@ inline cl_int copy(const CommandQueue& queue, const cl::Buffer& buffer, Iterator ::size_t byteLength = length * sizeof(DataType); DataType* pointer = - static_cast(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_READ, 0, byteLength, 0, 0, &error)); + static_cast(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_READ, 0, byteLength, nullptr, nullptr, &error)); // if exceptions enabled, enqueueMapBuffer will throw if (error != CL_SUCCESS) { From 751f54990c35ebc31363aea9f4b8b6828fec1685 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 01:28:30 +0200 Subject: [PATCH 04/64] Introduce gsl::span. Bound checking at compile time, no overhead at runtime See https://github.com/isocpp/CppCoreGuidelines/blob/master/CppCoreGuidelines.md --- .../adapters/beidou_b1i_pcps_acquisition.cc | 5 +- .../adapters/beidou_b3i_pcps_acquisition.cc | 5 +- ...lileo_e1_pcps_8ms_ambiguous_acquisition.cc | 6 +- .../galileo_e1_pcps_ambiguous_acquisition.cc | 14 +- ...ileo_e1_pcps_ambiguous_acquisition_fpga.cc | 8 +- ...eo_e1_pcps_cccwsr_ambiguous_acquisition.cc | 12 +- ...e1_pcps_quicksync_ambiguous_acquisition.cc | 6 +- ...ileo_e1_pcps_tong_ambiguous_acquisition.cc | 7 +- ...ileo_e5a_noncoherent_iq_acquisition_caf.cc | 14 +- .../adapters/galileo_e5a_pcps_acquisition.cc | 16 +- .../galileo_e5a_pcps_acquisition_fpga.cc | 12 +- .../glonass_l1_ca_pcps_acquisition.cc | 4 +- .../glonass_l2_ca_pcps_acquisition.cc | 4 +- .../adapters/gps_l1_ca_pcps_acquisition.cc | 7 +- ...gps_l1_ca_pcps_acquisition_fine_doppler.cc | 4 +- .../gps_l1_ca_pcps_acquisition_fpga.cc | 4 +- .../gps_l1_ca_pcps_assisted_acquisition.cc | 4 +- .../gps_l1_ca_pcps_opencl_acquisition.cc | 4 +- .../gps_l1_ca_pcps_quicksync_acquisition.cc | 4 +- .../gps_l1_ca_pcps_tong_acquisition.cc | 4 +- .../adapters/gps_l2_m_pcps_acquisition.cc | 9 +- .../gps_l2_m_pcps_acquisition_fpga.cc | 4 +- .../adapters/gps_l5i_pcps_acquisition.cc | 6 +- .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 2 +- .../gnuradio_blocks/pcps_acquisition.cc | 12 +- .../pcps_acquisition_fine_doppler_cc.cc | 6 +- src/algorithms/libs/CMakeLists.txt | 9 + .../libs/beidou_b1i_signal_processing.cc | 12 +- .../libs/beidou_b1i_signal_processing.h | 17 +- .../libs/beidou_b3i_signal_processing.cc | 8 +- .../libs/beidou_b3i_signal_processing.h | 17 +- .../libs/galileo_e1_signal_processing.cc | 49 +- .../libs/galileo_e1_signal_processing.h | 18 +- .../libs/galileo_e5_signal_processing.cc | 8 +- .../libs/galileo_e5_signal_processing.h | 15 +- .../libs/glonass_l1_signal_processing.cc | 10 +- .../libs/glonass_l1_signal_processing.h | 13 +- .../libs/glonass_l2_signal_processing.cc | 10 +- .../libs/glonass_l2_signal_processing.h | 13 +- src/algorithms/libs/gnss_signal_processing.cc | 24 +- src/algorithms/libs/gnss_signal_processing.h | 25 +- src/algorithms/libs/gps_l2c_signal.cc | 14 +- src/algorithms/libs/gps_l2c_signal.h | 13 +- src/algorithms/libs/gps_l5_signal.cc | 28 +- src/algorithms/libs/gps_l5_signal.h | 19 +- .../libs/gps_sdr_signal_processing.cc | 8 +- .../libs/gps_sdr_signal_processing.h | 17 +- src/algorithms/libs/gsl/include/gsl/gsl | 29 + .../libs/gsl/include/gsl/gsl_algorithm | 61 + .../libs/gsl/include/gsl/gsl_assert | 177 ++ src/algorithms/libs/gsl/include/gsl/gsl_byte | 203 ++ src/algorithms/libs/gsl/include/gsl/gsl_util | 175 ++ .../libs/gsl/include/gsl/multi_span | 2293 +++++++++++++++++ src/algorithms/libs/gsl/include/gsl/pointers | 294 +++ src/algorithms/libs/gsl/include/gsl/span | 793 ++++++ .../libs/gsl/include/gsl/string_span | 722 ++++++ .../gnuradio_blocks/signal_generator_c.cc | 14 +- .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 10 +- .../galileo_e5a_dll_pll_tracking_fpga.cc | 3 +- .../gps_l1_ca_dll_pll_tracking_fpga.cc | 2 +- .../gps_l2_m_dll_pll_tracking_fpga.cc | 2 +- .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 6 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 31 +- .../galileo_e1_tcp_connector_tracking_cc.cc | 6 +- ...glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 2 +- ...glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 2 +- .../glonass_l1_ca_dll_pll_tracking_cc.cc | 2 +- ...glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc | 2 +- ...glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc | 2 +- .../glonass_l2_ca_dll_pll_tracking_cc.cc | 2 +- .../gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 2 +- .../gps_l1_ca_kf_tracking_cc.cc | 3 +- .../gps_l1_ca_tcp_connector_tracking_cc.cc | 2 +- .../arithmetic/code_generation_test.cc | 12 +- .../arithmetic/complex_carrier_test.cc | 8 +- .../hybrid_observables_test_fpga.cc | 20 +- .../cpu_multicorrelator_real_codes_test.cc | 2 +- .../tracking/cpu_multicorrelator_test.cc | 2 +- .../tracking/tracking_pull-in_test_fpga.cc | 31 +- 79 files changed, 5148 insertions(+), 297 deletions(-) create mode 100644 src/algorithms/libs/gsl/include/gsl/gsl create mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_algorithm create mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_assert create mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_byte create mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_util create mode 100644 src/algorithms/libs/gsl/include/gsl/multi_span create mode 100644 src/algorithms/libs/gsl/include/gsl/pointers create mode 100644 src/algorithms/libs/gsl/include/gsl/span create mode 100644 src/algorithms/libs/gsl/include/gsl/string_span diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc index 7317dbc0b..c4d188771 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc @@ -123,7 +123,7 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -206,7 +206,7 @@ void BeidouB1iPcpsAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - beidou_b1i_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + beidou_b1i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); for (uint32_t i = 0; i < sampled_ms_; i++) { @@ -331,6 +331,7 @@ gr::basic_block_sptr BeidouB1iPcpsAcquisition::get_right_block() return acquisition_; } + void BeidouB1iPcpsAcquisition::set_resampler_latency(uint32_t latency_samples) { acquisition_->set_resampler_latency(latency_samples); diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc index 6855d11fb..f92b61572 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc @@ -121,7 +121,7 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -143,6 +143,7 @@ void BeidouB3iPcpsAcquisition::stop_acquisition() { } + void BeidouB3iPcpsAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + ".pfa", 0.0); @@ -203,7 +204,7 @@ void BeidouB3iPcpsAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - beidou_b3i_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + beidou_b3i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); for (unsigned int i = 0; i < sampled_ms_; i++) { diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc index 6c0f61c30..ddfeb7b8a 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc @@ -110,7 +110,7 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -217,8 +217,10 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_local_code() "Acquisition" + std::to_string(channel_) + ".cboc", false); auto* code = new std::complex[code_length_]; + std::array Signal_; + std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index ea1b9ea73..d419cd0fb 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -154,7 +154,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -247,28 +247,30 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() if (acquire_pilot_ == true) { //set local signal generator to Galileo E1 pilot component (1C) - char pilot_signal[3] = "1C"; + std::array pilot_signal = {{'1', 'C', '\0'}}; if (acq_parameters_.use_automatic_resampler) { - galileo_e1_code_gen_complex_sampled(code, pilot_signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), pilot_signal, cboc, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0, false); } else { - galileo_e1_code_gen_complex_sampled(code, pilot_signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), pilot_signal, cboc, gnss_synchro_->PRN, fs_in_, 0, false); } } else { + std::array Signal_; + std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); if (acq_parameters_.use_automatic_resampler) { - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0, false); } else { - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); } } diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index e7a45bfe7..b781319b0 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -122,14 +122,14 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( if (acquire_pilot_ == true) { //set local signal generator to Galileo E1 pilot component (1C) - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_complex_sampled(code, pilot_signal, + std::array pilot_signal = {{'1', 'C', '\0'}}; + galileo_e1_code_gen_complex_sampled(gsl::span>(code, nsamples_total), pilot_signal, cboc, PRN, fs_in, 0, false); } else { - char data_signal[3] = "1B"; - galileo_e1_code_gen_complex_sampled(code, data_signal, + std::array data_signal = {{'1', 'B', '\0'}}; + galileo_e1_code_gen_complex_sampled(gsl::span>(code, nsamples_total), data_signal, cboc, PRN, fs_in, 0, false); } diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc index 1375a22a8..d29e896a5 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc @@ -111,7 +111,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -203,16 +203,14 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_local_code() bool cboc = configuration_->property( "Acquisition" + std::to_string(channel_) + ".cboc", false); - char signal[3]; + std::array signal = {{'1', 'B', '\0'}}; - strcpy(signal, "1B"); - - galileo_e1_code_gen_complex_sampled(code_data_, signal, + galileo_e1_code_gen_complex_sampled(gsl::span(code_data_, vector_length_), signal, cboc, gnss_synchro_->PRN, fs_in_, 0, false); - strcpy(signal, "1C"); + std::array signal_C = {{'1', 'C', '\0'}}; - galileo_e1_code_gen_complex_sampled(code_pilot_, signal, + galileo_e1_code_gen_complex_sampled(gsl::span(code_pilot_, vector_length_), signal_C, cboc, gnss_synchro_->PRN, fs_in_, 0, false); acquisition_cc_->set_local_code(code_data_, code_pilot_); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc index a64b3abda..8bc3f969c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc @@ -144,7 +144,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -251,8 +251,10 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code() "Acquisition" + std::to_string(channel_) + ".cboc", false); auto* code = new std::complex[code_length_]; + std::array Signal_; + std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc index 6bc6b8ad3..7728988e7 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc @@ -114,7 +114,7 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -221,8 +221,9 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_local_code() "Acquisition" + std::to_string(channel_) + ".cboc", false); auto* code = new std::complex[code_length_]; - - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + std::array Signal_; + std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); + galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc index 3828c58e6..f4eb2024a 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc @@ -119,7 +119,7 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -228,18 +228,18 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_local_code() if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') { - char a[3]; - strcpy(a, "5I"); - galileo_e5_a_code_gen_complex_sampled(codeI, a, + std::array a = {{'5', 'I', '\0'}}; + galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeI, code_length_), a, gnss_synchro_->PRN, fs_in_, 0); - strcpy(a, "5Q"); - galileo_e5_a_code_gen_complex_sampled(codeQ, a, + std::array b = {{'5', 'Q', '\0'}}; + galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeQ, code_length_), b, gnss_synchro_->PRN, fs_in_, 0); } else { - galileo_e5_a_code_gen_complex_sampled(codeI, gnss_synchro_->Signal, + std::array signal_type_ = {{'5', 'X', '\0'}}; + galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeI, code_length_), signal_type_, gnss_synchro_->PRN, fs_in_, 0); } // WARNING: 3ms are coherently integrated. Secondary sequence (1,1,1) diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc index 20880ba15..5b42ff2f2 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc @@ -152,7 +152,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -236,28 +236,30 @@ void GalileoE5aPcpsAcquisition::init() void GalileoE5aPcpsAcquisition::set_local_code() { auto* code = new gr_complex[code_length_]; - char signal_[3]; + std::array signal_; + signal_[0] = '5'; + signal_[2] = '\0'; if (acq_iq_) { - strcpy(signal_, "5X"); + signal_[1] = 'X'; } else if (acq_pilot_) { - strcpy(signal_, "5Q"); + signal_[1] = 'Q'; } else { - strcpy(signal_, "5I"); + signal_[1] = 'I'; } if (acq_parameters_.use_automatic_resampler) { - galileo_e5_a_code_gen_complex_sampled(code, signal_, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0); + galileo_e5_a_code_gen_complex_sampled(gsl::span(code, code_length_), signal_, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0); } else { - galileo_e5_a_code_gen_complex_sampled(code, signal_, gnss_synchro_->PRN, fs_in_, 0); + galileo_e5_a_code_gen_complex_sampled(gsl::span(code, code_length_), signal_, gnss_synchro_->PRN, fs_in_, 0); } for (unsigned int i = 0; i < sampled_ms_; i++) diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index 70b8ac1dc..97a14f2a9 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -119,22 +119,24 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf for (uint32_t PRN = 1; PRN <= GALILEO_E5A_NUMBER_OF_CODES; PRN++) { - char signal_[3]; + std::array signal_; + signal_[0] = '5'; + signal_[2] = '\0'; if (acq_iq_) { - strcpy(signal_, "5X"); + signal_[1] = 'X'; } else if (acq_pilot_) { - strcpy(signal_, "5Q"); + signal_[1] = 'Q'; } else { - strcpy(signal_, "5I"); + signal_[1] = 'I'; } - galileo_e5_a_code_gen_complex_sampled(code, signal_, PRN, fs_in, 0); + galileo_e5_a_code_gen_complex_sampled(gsl::span>(code, nsamples_total), signal_, PRN, fs_in, 0); for (uint32_t s = code_length; s < 2 * code_length; s++) { diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc index c12ec73c2..a375d01f7 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -125,7 +125,7 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -208,7 +208,7 @@ void GlonassL1CaPcpsAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - glonass_l1_ca_code_gen_complex_sampled(code, /* gnss_synchro_->PRN,*/ fs_in_, 0); + glonass_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), /* gnss_synchro_->PRN,*/ fs_in_, 0); for (unsigned int i = 0; i < sampled_ms_; i++) { diff --git a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc index 0b7056da3..0fd369a9b 100644 --- a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc @@ -124,7 +124,7 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -208,7 +208,7 @@ void GlonassL2CaPcpsAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - glonass_l2_ca_code_gen_complex_sampled(code, /* gnss_synchro_->PRN,*/ fs_in_, 0); + glonass_l2_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), /* gnss_synchro_->PRN,*/ fs_in_, 0); for (unsigned int i = 0; i < sampled_ms_; i++) { diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc index 0c6b9b2d5..4e458ebcc 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc @@ -41,6 +41,7 @@ #include "gps_sdr_signal_processing.h" #include #include +#include GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition( @@ -147,7 +148,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -230,11 +231,11 @@ void GpsL1CaPcpsAcquisition::set_local_code() if (acq_parameters_.use_automatic_resampler) { - gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0); } else { - gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); } for (unsigned int i = 0; i < sampled_ms_; i++) { diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc index 6dfcd6452..6a4cfa7fd 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc @@ -97,7 +97,7 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -163,7 +163,7 @@ void GpsL1CaPcpsAcquisitionFineDoppler::init() void GpsL1CaPcpsAcquisitionFineDoppler::set_local_code() { - gps_l1_ca_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code_, vector_length_), gnss_synchro_->PRN, fs_in_, 0); acquisition_cc_->set_local_code(code_); } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index 7f0458300..b13f7ac23 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -113,7 +113,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // temporary maxima search for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l1_ca_code_gen_complex_sampled(code, PRN, fs_in, 0); // generate PRN code + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, nsamples_total), PRN, fs_in, 0); // generate PRN code for (uint32_t s = code_length; s < 2 * code_length; s++) { @@ -169,7 +169,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( channel_ = 0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + // temporary buffers that we can delete delete[] code; delete fft_if; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc index efe1dcb1a..d603eb216 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc @@ -89,7 +89,7 @@ GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -154,7 +154,7 @@ void GpsL1CaPcpsAssistedAcquisition::init() void GpsL1CaPcpsAssistedAcquisition::set_local_code() { - gps_l1_ca_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span(code_, vector_length_), gnss_synchro_->PRN, fs_in_, 0); acquisition_cc_->set_local_code(code_); } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc index 90abee071..b0fb13060 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc @@ -105,7 +105,7 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -209,7 +209,7 @@ void GpsL1CaPcpsOpenClAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); for (unsigned int i = 0; i < sampled_ms_; i++) { diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc index 37a91768d..b1b0034f4 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc @@ -137,7 +137,7 @@ GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -238,7 +238,7 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_local_code() { auto* code = new std::complex[code_length_](); - gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); for (unsigned int i = 0; i < (sampled_ms_ / folding_factor_); i++) { diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc index b81877010..7b56356bb 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc @@ -99,7 +99,7 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -199,7 +199,7 @@ void GpsL1CaPcpsTongAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); for (unsigned int i = 0; i < sampled_ms_; i++) { diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc index 6e529684c..d92c49102 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc @@ -151,7 +151,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + num_codes_ = acq_parameters_.sampled_ms / acq_parameters_.ms_per_code; if (in_streams_ > 1) { @@ -241,17 +241,15 @@ void GpsL2MPcpsAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - if (acq_parameters_.use_automatic_resampler) { - gps_l2c_m_code_gen_complex_sampled(code, gnss_synchro_->PRN, acq_parameters_.resampled_fs); + gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); } else { - gps_l2c_m_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_); + gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_); } - for (unsigned int i = 0; i < num_codes_; i++) { memcpy(&(code_[i * code_length_]), code, @@ -268,6 +266,7 @@ void GpsL2MPcpsAcquisition::reset() acquisition_->set_active(true); } + void GpsL2MPcpsAcquisition::set_state(int state) { acquisition_->set_state(state); diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index ac9517db2..e369189e9 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -113,7 +113,7 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l2c_m_code_gen_complex_sampled(code, PRN, fs_in_); + gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, nsamples_total), PRN, fs_in_); // fill in zero padding for (unsigned int s = code_length; s < nsamples_total; s++) { @@ -161,7 +161,7 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( channel_ = 0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + threshold_ = 0.0; } diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc index 7a587598a..204d5532e 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc @@ -147,7 +147,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( threshold_ = 0.0; doppler_step_ = 0; gnss_synchro_ = nullptr; - + if (in_streams_ > 1) { LOG(ERROR) << "This implementation only supports one input stream"; @@ -237,11 +237,11 @@ void GpsL5iPcpsAcquisition::set_local_code() if (acq_parameters_.use_automatic_resampler) { - gps_l5i_code_gen_complex_sampled(code, gnss_synchro_->PRN, acq_parameters_.resampled_fs); + gps_l5i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); } else { - gps_l5i_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_); + gps_l5i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_); } for (unsigned int i = 0; i < num_codes_; i++) diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index 1a8f37ba2..072abefb4 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -118,7 +118,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l5i_code_gen_complex_sampled(code, PRN, fs_in); + gps_l5i_code_gen_complex_sampled(gsl::span(code, nsamples_total), PRN, fs_in); for (uint32_t s = code_length; s < 2 * code_length; s++) { diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 3cf3564d5..c336587cb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -450,7 +450,7 @@ void pcps_acquisition::send_positive_acquisition() if (!d_channel_fsm.expired()) { - //the channel FSM is set, so, notify it directly the positive acquisition to minimize delays + // the channel FSM is set, so, notify it directly the positive acquisition to minimize delays d_channel_fsm.lock()->Event_valid_acquisition(); } else @@ -510,11 +510,11 @@ void pcps_acquisition::dump_results(int32_t effective_fft_size) dims[0] = static_cast(1); dims[1] = static_cast(1); - matvar = Mat_VarCreate("doppler_max", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &acq_parameters.doppler_max, 0); + matvar = Mat_VarCreate("doppler_max", MAT_C_INT32, MAT_T_INT32, 1, dims, &acq_parameters.doppler_max, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("doppler_step", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_doppler_step, 0); + matvar = Mat_VarCreate("doppler_step", MAT_C_INT32, MAT_T_INT32, 1, dims, &d_doppler_step, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -552,7 +552,7 @@ void pcps_acquisition::dump_results(int32_t effective_fft_size) Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("num_dwells", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_num_noncoherent_integrations_counter, 0); + matvar = Mat_VarCreate("num_dwells", MAT_C_INT32, MAT_T_INT32, 1, dims, &d_num_noncoherent_integrations_counter, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -774,7 +774,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) } if (acq_parameters.use_automatic_resampler) { - //take into account the acquisition resampler ratio + // take into account the acquisition resampler ratio d_gnss_synchro->Acq_delay_samples = static_cast(std::fmod(static_cast(indext), acq_parameters.samples_per_code)) * acq_parameters.resampler_ratio; d_gnss_synchro->Acq_delay_samples -= static_cast(acq_parameters.resampler_latency_samples); //account the resampler filter latency d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); @@ -832,7 +832,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) if (acq_parameters.use_automatic_resampler) { - //take into account the acquisition resampler ratio + // take into account the acquisition resampler ratio d_gnss_synchro->Acq_delay_samples = static_cast(std::fmod(static_cast(indext), acq_parameters.samples_per_code)) * acq_parameters.resampler_ratio; d_gnss_synchro->Acq_delay_samples -= static_cast(acq_parameters.resampler_latency_samples); //account the resampler filter latency d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index aa5af66c9..07afa1d60 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -430,7 +430,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler() //1. generate local code aligned with the acquisition code phase estimation auto *code_replica = static_cast(volk_gnsssdr_malloc(signal_samples * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0); + gps_l1_ca_code_gen_complex_sampled(gsl::span(code_replica, signal_samples * sizeof(gr_complex)), d_gnss_synchro->PRN, d_fs_in, 0); int shift_index = static_cast(d_gnss_synchro->Acq_delay_samples); @@ -705,11 +705,11 @@ void pcps_acquisition_fine_doppler_cc::dump_results(int effective_fft_size) dims[0] = static_cast(1); dims[1] = static_cast(1); - matvar = Mat_VarCreate("doppler_max", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_config_doppler_max, 0); + matvar = Mat_VarCreate("doppler_max", MAT_C_INT32, MAT_T_INT32, 1, dims, &d_config_doppler_max, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("doppler_step", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_doppler_step, 0); + matvar = Mat_VarCreate("doppler_step", MAT_C_INT32, MAT_T_INT32, 1, dims, &d_doppler_step, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index ca41943fb..f48579047 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -92,6 +92,15 @@ else() target_link_libraries(algorithms_libs PRIVATE Boost::filesystem Boost::system) endif() +if(NOT (CMAKE_CXX_STANDARD VERSION_LESS 20)) + target_compile_definitions(algorithms_libs PUBLIC -DHAS_SPAN=1) +else() + target_include_directories(algorithms_libs + PUBLIC + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/gsl/include + ) +endif() + target_link_libraries(algorithms_libs PUBLIC Armadillo::armadillo diff --git a/src/algorithms/libs/beidou_b1i_signal_processing.cc b/src/algorithms/libs/beidou_b1i_signal_processing.cc index 61e1ffa47..570c1e03a 100644 --- a/src/algorithms/libs/beidou_b1i_signal_processing.cc +++ b/src/algorithms/libs/beidou_b1i_signal_processing.cc @@ -34,7 +34,7 @@ auto auxCeil = [](float x) { return static_cast(static_cast((x) + 1)); }; -void beidou_b1i_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift) +void beidou_b1i_code_gen_int(gsl::span _dest, int32_t _prn, uint32_t _chip_shift) { const uint32_t _code_length = 2046; bool G1[_code_length]; @@ -112,12 +112,12 @@ void beidou_b1i_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift) } -void beidou_b1i_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift) +void beidou_b1i_code_gen_float(gsl::span _dest, int32_t _prn, uint32_t _chip_shift) { uint32_t _code_length = 2046; int32_t b1i_code_int[_code_length]; - beidou_b1i_code_gen_int(b1i_code_int, _prn, _chip_shift); + beidou_b1i_code_gen_int(gsl::span(b1i_code_int, _code_length), _prn, _chip_shift); for (uint32_t ii = 0; ii < _code_length; ++ii) { @@ -126,12 +126,12 @@ void beidou_b1i_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift) } -void beidou_b1i_code_gen_complex(std::complex* _dest, int32_t _prn, uint32_t _chip_shift) +void beidou_b1i_code_gen_complex(gsl::span> _dest, int32_t _prn, uint32_t _chip_shift) { uint32_t _code_length = 2046; int32_t b1i_code_int[_code_length]; - beidou_b1i_code_gen_int(b1i_code_int, _prn, _chip_shift); + beidou_b1i_code_gen_int(gsl::span(b1i_code_int, _code_length), _prn, _chip_shift); for (uint32_t ii = 0; ii < _code_length; ++ii) { @@ -143,7 +143,7 @@ void beidou_b1i_code_gen_complex(std::complex* _dest, int32_t _prn, uint3 /* * Generates complex GPS L1 C/A code for the desired SV ID and sampled to specific sampling frequency */ -void beidou_b1i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) +void beidou_b1i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { // This function is based on the GNU software GPS for MATLAB in the Kay Borre book std::complex _code[2046]; diff --git a/src/algorithms/libs/beidou_b1i_signal_processing.h b/src/algorithms/libs/beidou_b1i_signal_processing.h index e653dc3cf..367a6d7b1 100644 --- a/src/algorithms/libs/beidou_b1i_signal_processing.h +++ b/src/algorithms/libs/beidou_b1i_signal_processing.h @@ -36,20 +36,27 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //! Generates int32_t GPS L1 C/A code for the desired SV ID and code shift -void beidou_b1i_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift); +void beidou_b1i_code_gen_int(gsl::span _dest, int32_t _prn, uint32_t _chip_shift); //! Generates float GPS L1 C/A code for the desired SV ID and code shift -void beidou_b1i_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift); +void beidou_b1i_code_gen_float(gsl::span _dest, int32_t _prn, uint32_t _chip_shift); //! Generates complex GPS L1 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency -void beidou_b1i_code_gen_complex(std::complex* _dest, int32_t _prn, uint32_t _chip_shift); +void beidou_b1i_code_gen_complex(gsl::span> _dest, int32_t _prn, uint32_t _chip_shift); //! Generates N complex GPS L1 C/A codes for the desired SV ID and code shift -void beidou_b1i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); +void beidou_b1i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); //! Generates complex GPS L1 C/A code for the desired SV ID and code shift -void beidou_b1i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); +void beidou_b1i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/beidou_b3i_signal_processing.cc b/src/algorithms/libs/beidou_b3i_signal_processing.cc index 9b29f8090..2c36fd64d 100644 --- a/src/algorithms/libs/beidou_b3i_signal_processing.cc +++ b/src/algorithms/libs/beidou_b3i_signal_processing.cc @@ -34,7 +34,7 @@ auto auxCeil = [](float x) { return static_cast(static_cast((x) + 1)); }; -void beidou_b3i_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shift) +void beidou_b3i_code_gen_int(gsl::span _dest, signed int _prn, unsigned int _chip_shift) { const unsigned int _code_length = 10230; bool G1[_code_length]; @@ -170,7 +170,7 @@ void beidou_b3i_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shi } -void beidou_b3i_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_shift) +void beidou_b3i_code_gen_float(gsl::span _dest, signed int _prn, unsigned int _chip_shift) { unsigned int _code_length = 10230; int b3i_code_int[10230]; @@ -184,7 +184,7 @@ void beidou_b3i_code_gen_float(float* _dest, signed int _prn, unsigned int _chip } -void beidou_b3i_code_gen_complex(std::complex* _dest, signed int _prn, unsigned int _chip_shift) +void beidou_b3i_code_gen_complex(gsl::span> _dest, signed int _prn, unsigned int _chip_shift) { unsigned int _code_length = 10230; int b3i_code_int[10230]; @@ -198,7 +198,7 @@ void beidou_b3i_code_gen_complex(std::complex* _dest, signed int _prn, un } -void beidou_b3i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift) +void beidou_b3i_code_gen_complex_sampled(gsl::span> _dest, unsigned int _prn, int _fs, unsigned int _chip_shift) { // This function is based on the GNU software GPS for MATLAB in the Kay Borre book std::complex _code[10230]; diff --git a/src/algorithms/libs/beidou_b3i_signal_processing.h b/src/algorithms/libs/beidou_b3i_signal_processing.h index 1bfab5519..7093407ce 100644 --- a/src/algorithms/libs/beidou_b3i_signal_processing.h +++ b/src/algorithms/libs/beidou_b3i_signal_processing.h @@ -39,19 +39,26 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //! Generates int BeiDou B3I code for the desired SV ID and code shift -void beidou_b3i_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shift); +void beidou_b3i_code_gen_int(gsl::span _dest, signed int _prn, unsigned int _chip_shift); //! Generates float BeiDou B3I code for the desired SV ID and code shift -void beidou_b3i_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_shift); +void beidou_b3i_code_gen_float(gsl::span _dest, signed int _prn, unsigned int _chip_shift); //! Generates complex BeiDou B3I code for the desired SV ID and code shift, and sampled to specific sampling frequency -void beidou_b3i_code_gen_complex(std::complex* _dest, signed int _prn, unsigned int _chip_shift); +void beidou_b3i_code_gen_complex(gsl::span> _dest, signed int _prn, unsigned int _chip_shift); //! Generates N complex BeiDou B3I codes for the desired SV ID and code shift -void beidou_b3i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift, unsigned int _ncodes); +void beidou_b3i_code_gen_complex_sampled(gsl::span> _dest, unsigned int _prn, int _fs, unsigned int _chip_shift, unsigned int _ncodes); //! Generates complex BeiDou B3I code for the desired SV ID and code shift -void beidou_b3i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift); +void beidou_b3i_code_gen_complex_sampled(gsl::span> _dest, unsigned int _prn, int _fs, unsigned int _chip_shift); #endif /* GNSS_SDR_BEIDOU_B3I_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index d3e92c804..2b4a6a527 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -37,9 +37,9 @@ #include -void galileo_e1_code_gen_int(int* _dest, char _Signal[3], int32_t _prn) +void galileo_e1_code_gen_int(gsl::span _dest, std::array _Signal, int32_t _prn) { - std::string _galileo_signal = _Signal; + std::string _galileo_signal = _Signal.data(); int32_t prn = _prn - 1; int32_t index = 0; @@ -68,10 +68,10 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], int32_t _prn) } -void galileo_e1_sinboc_11_gen_int(int* _dest, const int* _prn, uint32_t _length_out) +void galileo_e1_sinboc_11_gen_int(gsl::span _dest, gsl::span _prn) { const uint32_t _length_in = GALILEO_E1_B_CODE_LENGTH_CHIPS; - auto _period = static_cast(_length_out / _length_in); + auto _period = static_cast(_dest.size() / _length_in); for (uint32_t i = 0; i < _length_in; i++) { for (uint32_t j = 0; j < (_period / 2); j++) @@ -86,10 +86,10 @@ void galileo_e1_sinboc_11_gen_int(int* _dest, const int* _prn, uint32_t _length_ } -void galileo_e1_sinboc_61_gen_int(int* _dest, const int* _prn, uint32_t _length_out) +void galileo_e1_sinboc_61_gen_int(gsl::span _dest, gsl::span _prn) { const uint32_t _length_in = GALILEO_E1_B_CODE_LENGTH_CHIPS; - auto _period = static_cast(_length_out / _length_in); + auto _period = static_cast(_dest.size() / _length_in); for (uint32_t i = 0; i < _length_in; i++) { @@ -105,9 +105,9 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, const int* _prn, uint32_t _length_ } -void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], uint32_t _prn) +void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, std::array _Signal, uint32_t _prn) { - std::string _galileo_signal = _Signal; + std::string _galileo_signal = _Signal.data(); const auto _codeLength = static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS); int32_t primary_code_E1_chips[4092]; // _codeLength not accepted by Clang galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip @@ -119,18 +119,20 @@ void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], uint32_t } -void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3]) +void galileo_e1_gen_float(gsl::span _dest, gsl::span _prn, std::array _Signal) { - std::string _galileo_signal = _Signal; + std::string _galileo_signal = _Signal.data(); const uint32_t _codeLength = 12 * GALILEO_E1_B_CODE_LENGTH_CHIPS; const float alpha = sqrt(10.0 / 11.0); const float beta = sqrt(1.0 / 11.0); int32_t sinboc_11[12 * 4092] = {0}; // _codeLength not accepted by Clang int32_t sinboc_61[12 * 4092] = {0}; + gsl::span sinboc_11_(sinboc_11, _codeLength); + gsl::span sinboc_61_(sinboc_61, _codeLength); - galileo_e1_sinboc_11_gen_int(sinboc_11, _prn, _codeLength); //generate sinboc(1,1) 12 samples per chip - galileo_e1_sinboc_61_gen_int(sinboc_61, _prn, _codeLength); //generate sinboc(6,1) 12 samples per chip + galileo_e1_sinboc_11_gen_int(sinboc_11_, _prn); //generate sinboc(1,1) 12 samples per chip + galileo_e1_sinboc_61_gen_int(sinboc_61_, _prn); //generate sinboc(6,1) 12 samples per chip if (_galileo_signal.rfind("1B") != std::string::npos && _galileo_signal.length() >= 2) { @@ -151,12 +153,12 @@ void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3]) } -void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], +void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag) { // This function is based on the GNU software GPS for MATLAB in Kay Borre's book - std::string _galileo_signal = _Signal; + std::string _galileo_signal = _Signal.data(); uint32_t _samplesPerCode; const int32_t _codeFreqBasis = GALILEO_E1_CODE_CHIP_RATE_HZ; // Hz auto _codeLength = static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS); @@ -167,7 +169,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], const uint32_t delay = ((static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) - _chip_shift) % static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)) * _samplesPerCode / GALILEO_E1_B_CODE_LENGTH_CHIPS; - galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); // generate Galileo E1 code, 1 sample per chip + galileo_e1_code_gen_int(gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)), _Signal, _prn); // generate Galileo E1 code, 1 sample per chip float* _signal_E1; @@ -176,12 +178,12 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], if (_cboc == true) { - galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal); // generate cboc 12 samples per chip + galileo_e1_gen_float(gsl::span(_signal_E1, _codeLength), gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)), _Signal); // generate cboc 12 samples per chip } else { auto* _signal_E1_int = static_cast(volk_gnsssdr_malloc(_codeLength * sizeof(int32_t), volk_gnsssdr_get_alignment())); - galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength); // generate sinboc(1,1) 2 samples per chip + galileo_e1_sinboc_11_gen_int(gsl::span(_signal_E1_int, _codeLength), gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS))); // generate sinboc(1,1) 2 samples per chip for (uint32_t ii = 0; ii < _codeLength; ++ii) { @@ -194,8 +196,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], { auto* _resampled_signal = new float[_samplesPerCode]; - resampler(_signal_E1, _resampled_signal, _samplesPerChip * _codeFreqBasis, _fs, - _codeLength, _samplesPerCode); // resamples code to fs + resampler(gsl::span(_signal_E1, _codeLength), gsl::span(_resampled_signal, _samplesPerCode), _samplesPerChip * _codeFreqBasis, _fs); // resamples code to fs delete[] _signal_E1; _signal_E1 = _resampled_signal; @@ -229,11 +230,11 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], } -void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signal[3], +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag) { - std::string _galileo_signal = _Signal; + std::string _galileo_signal = _Signal.data(); const int32_t _codeFreqBasis = GALILEO_E1_CODE_CHIP_RATE_HZ; // Hz auto _samplesPerCode = static_cast(static_cast(_fs) / (static_cast(_codeFreqBasis) / static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS))); @@ -245,7 +246,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signa auto* real_code = static_cast(volk_gnsssdr_malloc(_samplesPerCode * sizeof(float), volk_gnsssdr_get_alignment())); - galileo_e1_code_gen_float_sampled(real_code, _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag); + galileo_e1_code_gen_float_sampled(gsl::span(real_code, _samplesPerCode), _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag); for (uint32_t ii = 0; ii < _samplesPerCode; ++ii) { @@ -255,14 +256,14 @@ void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signa } -void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], +void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { galileo_e1_code_gen_float_sampled(_dest, _Signal, _cboc, _prn, _fs, _chip_shift, false); } -void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signal[3], +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { galileo_e1_code_gen_complex_sampled(_dest, _Signal, _cboc, _prn, _fs, _chip_shift, false); diff --git a/src/algorithms/libs/galileo_e1_signal_processing.h b/src/algorithms/libs/galileo_e1_signal_processing.h index 6784e6f22..2c9c03d1d 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.h +++ b/src/algorithms/libs/galileo_e1_signal_processing.h @@ -32,21 +32,29 @@ #ifndef GNSS_SDR_GALILEO_E1_SIGNAL_PROCESSING_H_ #define GNSS_SDR_GALILEO_E1_SIGNAL_PROCESSING_H_ +#include #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + /*! * \brief This function generates Galileo E1 code (can select E1B or E1C sinboc). * */ -void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], uint32_t _prn); +void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, std::array _Signal, uint32_t _prn); /*! * \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc * and the sample frequency _fs). * */ -void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], +void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag); @@ -55,7 +63,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], * and the sample frequency _fs). * */ -void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], +void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); /*! @@ -63,14 +71,14 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], * and the sample frequency _fs). * */ -void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signal[3], +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag); /*! * \brief galileo_e1_code_gen_complex_sampled without _secondary_flag for backward compatibility. */ -void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signal[3], +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GALILEO_E1_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/galileo_e5_signal_processing.cc b/src/algorithms/libs/galileo_e5_signal_processing.cc index d02508f50..f9a7a0442 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.cc +++ b/src/algorithms/libs/galileo_e5_signal_processing.cc @@ -37,7 +37,7 @@ #include -void galileo_e5_a_code_gen_complex_primary(std::complex* _dest, int32_t _prn, const char _Signal[3]) +void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, std::array _Signal) { uint32_t prn = _prn - 1; uint32_t index = 0; @@ -100,7 +100,7 @@ void galileo_e5_a_code_gen_complex_primary(std::complex* _dest, int32_t _ } -void galileo_e5_a_code_gen_complex_sampled(std::complex* _dest, char _Signal[3], +void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { uint32_t _samplesPerCode; @@ -110,7 +110,7 @@ void galileo_e5_a_code_gen_complex_sampled(std::complex* _dest, char _Sig auto* _code = new std::complex[_codeLength](); - galileo_e5_a_code_gen_complex_primary(_code, _prn, _Signal); + galileo_e5_a_code_gen_complex_primary(gsl::span>(_code, _codeLength), _prn, _Signal); _samplesPerCode = static_cast(static_cast(_fs) / (static_cast(_codeFreqBasis) / static_cast(_codeLength))); @@ -122,7 +122,7 @@ void galileo_e5_a_code_gen_complex_sampled(std::complex* _dest, char _Sig if (posix_memalign(reinterpret_cast(&_resampled_signal), 16, _samplesPerCode * sizeof(gr_complex)) == 0) { }; - resampler(_code, _resampled_signal, _codeFreqBasis, _fs, _codeLength, _samplesPerCode); // resamples code to fs + resampler(gsl::span>(_code, _codeLength), gsl::span>(_resampled_signal, _samplesPerCode), _codeFreqBasis, _fs); // resamples code to fs delete[] _code; _code = _resampled_signal; } diff --git a/src/algorithms/libs/galileo_e5_signal_processing.h b/src/algorithms/libs/galileo_e5_signal_processing.h index d9b6f5b9f..49235bd32 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.h +++ b/src/algorithms/libs/galileo_e5_signal_processing.h @@ -36,22 +36,27 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + /*! * \brief Generates Galileo E5a code at 1 sample/chip * bool _pilot generates E5aQ code if true and E5aI (data signal) if false. */ -void galileo_e5_a_code_gen_complex_primary(std::complex* _dest, int32_t _prn, const char _Signal[3]); - -void galileo_e5_a_code_gen_tiered(std::complex* _dest, std::complex* _primary, uint32_t _prn, char _Signal[3]); +void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, std::array _Signal); /*! * \brief Generates Galileo E5a complex code, shifted to the desired chip and sampled at a frequency fs * bool _pilot generates E5aQ code if true and E5aI (data signal) if false. */ -void galileo_e5_a_code_gen_complex_sampled(std::complex* _dest, - char _Signal[3], uint32_t _prn, int32_t _fs, uint32_t _chip_shift); +void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, + std::array _Signal, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GALILEO_E5_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/glonass_l1_signal_processing.cc b/src/algorithms/libs/glonass_l1_signal_processing.cc index 8707985b4..bda5cdf02 100644 --- a/src/algorithms/libs/glonass_l1_signal_processing.cc +++ b/src/algorithms/libs/glonass_l1_signal_processing.cc @@ -34,7 +34,7 @@ auto auxCeil = [](float x) { return static_cast(static_cast((x) + 1)); }; -void glonass_l1_ca_code_gen_complex(std::complex* _dest, /* int32_t _prn,*/ uint32_t _chip_shift) +void glonass_l1_ca_code_gen_complex(gsl::span> _dest, /* int32_t _prn,*/ uint32_t _chip_shift) { const uint32_t _code_length = 511; bool G1[_code_length]; @@ -104,7 +104,7 @@ void glonass_l1_ca_code_gen_complex(std::complex* _dest, /* int32_t _prn /* * Generates complex GLONASS L1 C/A code for the desired SV ID and sampled to specific sampling frequency */ -void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift) +void glonass_l1_ca_code_gen_complex_sampled(gsl::span> _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift) { // This function is based on the GNU software GPS for MATLAB in the Kay Borre book std::complex _code[511]; @@ -119,9 +119,9 @@ void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest, /* uint3 _samplesPerCode = static_cast(static_cast(_fs) / static_cast(_codeFreqBasis / _codeLength)); //--- Find time constants -------------------------------------------------- - _ts = 1.0 / static_cast(_fs); // Sampling period in sec - _tc = 1.0 / static_cast(_codeFreqBasis); // C/A chip period in sec - glonass_l1_ca_code_gen_complex(_code, _chip_shift); //generate C/A code 1 sample per chip + _ts = 1.0 / static_cast(_fs); // Sampling period in sec + _tc = 1.0 / static_cast(_codeFreqBasis); // C/A chip period in sec + glonass_l1_ca_code_gen_complex(gsl::span>(_code, 511), _chip_shift); //generate C/A code 1 sample per chip for (int32_t i = 0; i < _samplesPerCode; i++) { diff --git a/src/algorithms/libs/glonass_l1_signal_processing.h b/src/algorithms/libs/glonass_l1_signal_processing.h index 0b1a26cfc..854befe82 100644 --- a/src/algorithms/libs/glonass_l1_signal_processing.h +++ b/src/algorithms/libs/glonass_l1_signal_processing.h @@ -36,13 +36,20 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //!Generates complex GLONASS L1 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency -void glonass_l1_ca_code_gen_complex(std::complex* _dest, /*int32_t _prn,*/ uint32_t _chip_shift); +void glonass_l1_ca_code_gen_complex(gsl::span> _dest, /*int32_t _prn,*/ uint32_t _chip_shift); //! Generates N complex GLONASS L1 C/A codes for the desired SV ID and code shift -void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); +void glonass_l1_ca_code_gen_complex_sampled(gsl::span> _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); //! Generates complex GLONASS L1 C/A code for the desired SV ID and code shift -void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift); +void glonass_l1_ca_code_gen_complex_sampled(gsl::span> _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GLONASS_SDR_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/glonass_l2_signal_processing.cc b/src/algorithms/libs/glonass_l2_signal_processing.cc index 491b0e735..987e43d01 100644 --- a/src/algorithms/libs/glonass_l2_signal_processing.cc +++ b/src/algorithms/libs/glonass_l2_signal_processing.cc @@ -34,7 +34,7 @@ auto auxCeil = [](float x) { return static_cast(static_cast((x) + 1)); }; -void glonass_l2_ca_code_gen_complex(std::complex* _dest, /* int32_t _prn,*/ uint32_t _chip_shift) +void glonass_l2_ca_code_gen_complex(gsl::span> _dest, /* int32_t _prn,*/ uint32_t _chip_shift) { const uint32_t _code_length = 511; bool G1[_code_length]; @@ -104,7 +104,7 @@ void glonass_l2_ca_code_gen_complex(std::complex* _dest, /* int32_t _prn, /* * Generates complex GLONASS L2 C/A code for the desired SV ID and sampled to specific sampling frequency */ -void glonass_l2_ca_code_gen_complex_sampled(std::complex* _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift) +void glonass_l2_ca_code_gen_complex_sampled(gsl::span> _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift) { // This function is based on the GNU software GPS for MATLAB in the Kay Borre book std::complex _code[511]; @@ -119,9 +119,9 @@ void glonass_l2_ca_code_gen_complex_sampled(std::complex* _dest, /* uint3 _samplesPerCode = static_cast(static_cast(_fs) / static_cast(_codeFreqBasis / _codeLength)); //--- Find time constants -------------------------------------------------- - _ts = 1.0 / static_cast(_fs); // Sampling period in sec - _tc = 1.0 / static_cast(_codeFreqBasis); // C/A chip period in sec - glonass_l2_ca_code_gen_complex(_code, _chip_shift); //generate C/A code 1 sample per chip + _ts = 1.0 / static_cast(_fs); // Sampling period in sec + _tc = 1.0 / static_cast(_codeFreqBasis); // C/A chip period in sec + glonass_l2_ca_code_gen_complex(gsl::span>(_code, 511), _chip_shift); //generate C/A code 1 sample per chip for (int32_t i = 0; i < _samplesPerCode; i++) { diff --git a/src/algorithms/libs/glonass_l2_signal_processing.h b/src/algorithms/libs/glonass_l2_signal_processing.h index 7c798f45a..0f46d3e9d 100644 --- a/src/algorithms/libs/glonass_l2_signal_processing.h +++ b/src/algorithms/libs/glonass_l2_signal_processing.h @@ -36,13 +36,20 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //!Generates complex GLONASS L2 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency -void glonass_l2_ca_code_gen_complex(std::complex* _dest, /*int32_t _prn,*/ uint32_t _chip_shift); +void glonass_l2_ca_code_gen_complex(gsl::span> _dest, /*int32_t _prn,*/ uint32_t _chip_shift); //! Generates N complex GLONASS L2 C/A codes for the desired SV ID and code shift -void glonass_l2_ca_code_gen_complex_sampled(std::complex* _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); +void glonass_l2_ca_code_gen_complex_sampled(gsl::span> _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); //! Generates complex GLONASS L2 C/A code for the desired SV ID and code shift -void glonass_l2_ca_code_gen_complex_sampled(std::complex* _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift); +void glonass_l2_ca_code_gen_complex_sampled(gsl::span> _dest, /* uint32_t _prn,*/ int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GLONASS_L2_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/gnss_signal_processing.cc b/src/algorithms/libs/gnss_signal_processing.cc index 0dea461e7..b90007451 100644 --- a/src/algorithms/libs/gnss_signal_processing.cc +++ b/src/algorithms/libs/gnss_signal_processing.cc @@ -38,19 +38,19 @@ auto auxCeil2 = [](float x) { return static_cast(static_cast((x) + 1)); }; -void complex_exp_gen(std::complex* _dest, double _f, double _fs, uint32_t _samps) +void complex_exp_gen(gsl::span> _dest, double _f, double _fs) { gr::fxpt_nco d_nco; d_nco.set_freq((GPS_TWO_PI * _f) / _fs); - d_nco.sincos(_dest, _samps, 1); + d_nco.sincos(_dest.data(), _dest.size(), 1); } -void complex_exp_gen_conj(std::complex* _dest, double _f, double _fs, uint32_t _samps) +void complex_exp_gen_conj(gsl::span> _dest, double _f, double _fs) { gr::fxpt_nco d_nco; d_nco.set_freq(-(GPS_TWO_PI * _f) / _fs); - d_nco.sincos(_dest, _samps, 1); + d_nco.sincos(_dest.data(), _dest.size(), 1); } @@ -158,15 +158,15 @@ void hex_to_binary_converter(int32_t* _dest, char _from) } -void resampler(const float* _from, float* _dest, float _fs_in, - float _fs_out, uint32_t _length_in, uint32_t _length_out) +void resampler(const gsl::span _from, gsl::span _dest, float _fs_in, + float _fs_out) { uint32_t _codeValueIndex; float aux; //--- Find time constants -------------------------------------------------- const float _t_in = 1 / _fs_in; // Incoming sampling period in sec const float _t_out = 1 / _fs_out; // Out sampling period in sec - for (uint32_t i = 0; i < _length_out - 1; i++) + for (uint32_t i = 0; i < _dest.size() - 1; i++) { //=== Digitizing ======================================================= //--- compute index array to read sampled values ------------------------- @@ -178,19 +178,19 @@ void resampler(const float* _from, float* _dest, float _fs_in, _dest[i] = _from[_codeValueIndex]; } //--- Correct the last index (due to number rounding issues) ----------- - _dest[_length_out - 1] = _from[_length_in - 1]; + _dest[_dest.size() - 1] = _from[_from.size() - 1]; } -void resampler(const std::complex* _from, std::complex* _dest, float _fs_in, - float _fs_out, uint32_t _length_in, uint32_t _length_out) +void resampler(gsl::span> _from, gsl::span> _dest, float _fs_in, + float _fs_out) { uint32_t _codeValueIndex; float aux; //--- Find time constants -------------------------------------------------- const float _t_in = 1 / _fs_in; // Incoming sampling period in sec const float _t_out = 1 / _fs_out; // Out sampling period in sec - for (uint32_t i = 0; i < _length_out - 1; i++) + for (uint32_t i = 0; i < _dest.size() - 1; i++) { //=== Digitizing ======================================================= //--- compute index array to read sampled values ------------------------- @@ -202,5 +202,5 @@ void resampler(const std::complex* _from, std::complex* _dest, flo _dest[i] = _from[_codeValueIndex]; } //--- Correct the last index (due to number rounding issues) ----------- - _dest[_length_out - 1] = _from[_length_in - 1]; + _dest[_dest.size() - 1] = _from[_from.size() - 1]; } diff --git a/src/algorithms/libs/gnss_signal_processing.h b/src/algorithms/libs/gnss_signal_processing.h index 3ed363e3e..ba82a542b 100644 --- a/src/algorithms/libs/gnss_signal_processing.h +++ b/src/algorithms/libs/gnss_signal_processing.h @@ -38,21 +38,25 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + /*! * \brief This function generates a complex exponential in _dest. * */ -void complex_exp_gen(std::complex* _dest, double _f, double _fs, - uint32_t _samps); +void complex_exp_gen(gsl::span> _dest, double _f, double _fs); /*! * \brief This function generates a conjugate complex exponential in _dest. * */ -void complex_exp_gen_conj(std::complex* _dest, double _f, double _fs, - uint32_t _samps); - +void complex_exp_gen_conj(gsl::span> _dest, double _f, double _fs); /*! * \brief This function makes a conversion from hex (the input is a char) @@ -65,15 +69,14 @@ void hex_to_binary_converter(int32_t* _dest, char _from); * \brief This function resamples a sequence of float values. * */ -void resampler(const float* _from, float* _dest, - float _fs_in, float _fs_out, uint32_t _length_in, - uint32_t _length_out); +void resampler(const gsl::span _from, gsl::span _dest, + float _fs_in, float _fs_out); + /*! * \brief This function resamples a sequence of complex values. * */ -void resampler(const std::complex* _from, std::complex* _dest, - float _fs_in, float _fs_out, uint32_t _length_in, - uint32_t _length_out); +void resampler(gsl::span> _from, gsl::span> _dest, + float _fs_in, float _fs_out); #endif /* GNSS_SDR_GNSS_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/gps_l2c_signal.cc b/src/algorithms/libs/gps_l2c_signal.cc index d8995a1f2..e452072a5 100644 --- a/src/algorithms/libs/gps_l2c_signal.cc +++ b/src/algorithms/libs/gps_l2c_signal.cc @@ -41,7 +41,7 @@ int32_t gps_l2c_m_shift(int32_t x) } -void gps_l2c_m_code(int32_t* _dest, uint32_t _prn) +void gps_l2c_m_code(gsl::span _dest, uint32_t _prn) { int32_t x; x = GPS_L2C_M_INIT_REG[_prn - 1]; @@ -53,13 +53,13 @@ void gps_l2c_m_code(int32_t* _dest, uint32_t _prn) } -void gps_l2c_m_code_gen_complex(std::complex* _dest, uint32_t _prn) +void gps_l2c_m_code_gen_complex(gsl::span> _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - gps_l2c_m_code(_code, _prn); + gps_l2c_m_code(gsl::span(_code, GPS_L2_M_CODE_LENGTH_CHIPS), _prn); } for (int32_t i = 0; i < GPS_L2_M_CODE_LENGTH_CHIPS; i++) @@ -71,13 +71,13 @@ void gps_l2c_m_code_gen_complex(std::complex* _dest, uint32_t _prn) } -void gps_l2c_m_code_gen_float(float* _dest, uint32_t _prn) +void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - gps_l2c_m_code(_code, _prn); + gps_l2c_m_code(gsl::span(_code, GPS_L2_M_CODE_LENGTH_CHIPS), _prn); } for (int32_t i = 0; i < GPS_L2_M_CODE_LENGTH_CHIPS; i++) @@ -92,12 +92,12 @@ void gps_l2c_m_code_gen_float(float* _dest, uint32_t _prn) /* * Generates complex GPS L2C M code for the desired SV ID and sampled to specific sampling frequency */ -void gps_l2c_m_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs) +void gps_l2c_m_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - gps_l2c_m_code(_code, _prn); + gps_l2c_m_code(gsl::span(_code, GPS_L2_M_CODE_LENGTH_CHIPS), _prn); } int32_t _samplesPerCode, _codeValueIndex; diff --git a/src/algorithms/libs/gps_l2c_signal.h b/src/algorithms/libs/gps_l2c_signal.h index 84d51e485..af1aec8e6 100644 --- a/src/algorithms/libs/gps_l2c_signal.h +++ b/src/algorithms/libs/gps_l2c_signal.h @@ -36,12 +36,19 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //! Generates complex GPS L2C M code for the desired SV ID -void gps_l2c_m_code_gen_complex(std::complex* _dest, uint32_t _prn); -void gps_l2c_m_code_gen_float(float* _dest, uint32_t _prn); +void gps_l2c_m_code_gen_complex(gsl::span> _dest, uint32_t _prn); +void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn); //! Generates complex GPS L2C M code for the desired SV ID, and sampled to specific sampling frequency -void gps_l2c_m_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs); +void gps_l2c_m_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs); #endif /* GNSS_GPS_L2C_SIGNAL_H_ */ diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index c5fc6e13c..9383ec2b2 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -131,7 +131,7 @@ std::deque make_l5q_xb() } -void make_l5i(int32_t* _dest, int32_t prn) +void make_l5i(gsl::span _dest, int32_t prn) { int32_t xb_offset = GPS_L5I_INIT_REG[prn]; @@ -151,7 +151,7 @@ void make_l5i(int32_t* _dest, int32_t prn) } -void make_l5q(int32_t* _dest, int32_t prn) +void make_l5q(gsl::span _dest, int32_t prn) { int32_t xb_offset = GPS_L5Q_INIT_REG[prn]; @@ -171,13 +171,13 @@ void make_l5q(int32_t* _dest, int32_t prn) } -void gps_l5i_code_gen_complex(std::complex* _dest, uint32_t _prn) +void gps_l5i_code_gen_complex(gsl::span> _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5i(_code, _prn - 1); + make_l5i(gsl::span(_code, GPS_L5I_CODE_LENGTH_CHIPS), _prn - 1); } for (int32_t i = 0; i < GPS_L5I_CODE_LENGTH_CHIPS; i++) @@ -189,13 +189,13 @@ void gps_l5i_code_gen_complex(std::complex* _dest, uint32_t _prn) } -void gps_l5i_code_gen_float(float* _dest, uint32_t _prn) +void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5i(_code, _prn - 1); + make_l5i(gsl::span(_code, GPS_L5I_CODE_LENGTH_CHIPS), _prn - 1); } for (int32_t i = 0; i < GPS_L5I_CODE_LENGTH_CHIPS; i++) @@ -210,12 +210,12 @@ void gps_l5i_code_gen_float(float* _dest, uint32_t _prn) /* * Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency */ -void gps_l5i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs) +void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5i(_code, _prn - 1); + make_l5i(gsl::span(_code, GPS_L5I_CODE_LENGTH_CHIPS), _prn - 1); } int32_t _samplesPerCode, _codeValueIndex; @@ -252,13 +252,13 @@ void gps_l5i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, } -void gps_l5q_code_gen_complex(std::complex* _dest, uint32_t _prn) +void gps_l5q_code_gen_complex(gsl::span> _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5q(_code, _prn - 1); + make_l5q(gsl::span(_code, GPS_L5Q_CODE_LENGTH_CHIPS), _prn - 1); } for (int32_t i = 0; i < GPS_L5Q_CODE_LENGTH_CHIPS; i++) @@ -270,13 +270,13 @@ void gps_l5q_code_gen_complex(std::complex* _dest, uint32_t _prn) } -void gps_l5q_code_gen_float(float* _dest, uint32_t _prn) +void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5q(_code, _prn - 1); + make_l5q(gsl::span(_code, GPS_L5Q_CODE_LENGTH_CHIPS), _prn - 1); } for (int32_t i = 0; i < GPS_L5Q_CODE_LENGTH_CHIPS; i++) @@ -291,12 +291,12 @@ void gps_l5q_code_gen_float(float* _dest, uint32_t _prn) /* * Generates complex GPS L5Q code for the desired SV ID and sampled to specific sampling frequency */ -void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs) +void gps_l5q_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5q(_code, _prn - 1); + make_l5q(gsl::span(_code, GPS_L5Q_CODE_LENGTH_CHIPS), _prn - 1); } int32_t _samplesPerCode, _codeValueIndex; diff --git a/src/algorithms/libs/gps_l5_signal.h b/src/algorithms/libs/gps_l5_signal.h index de8e44c0d..e3b653797 100644 --- a/src/algorithms/libs/gps_l5_signal.h +++ b/src/algorithms/libs/gps_l5_signal.h @@ -36,23 +36,30 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //! Generates complex GPS L5I code for the desired SV ID -void gps_l5i_code_gen_complex(std::complex* _dest, uint32_t _prn); +void gps_l5i_code_gen_complex(gsl::span> _dest, uint32_t _prn); //! Generates real GPS L5I code for the desired SV ID -void gps_l5i_code_gen_float(float* _dest, uint32_t _prn); +void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn); //! Generates complex GPS L5Q code for the desired SV ID -void gps_l5q_code_gen_complex(std::complex* _dest, uint32_t _prn); +void gps_l5q_code_gen_complex(gsl::span> _dest, uint32_t _prn); //! Generates real GPS L5Q code for the desired SV ID -void gps_l5q_code_gen_float(float* _dest, uint32_t _prn); +void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn); //! Generates complex GPS L5I code for the desired SV ID, and sampled to specific sampling frequency -void gps_l5i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs); +void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs); //! Generates complex GPS L5Q code for the desired SV ID, and sampled to specific sampling frequency -void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs); +void gps_l5q_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs); #endif /* GNSS_SDR_GPS_L5_SIGNAL_H_ */ diff --git a/src/algorithms/libs/gps_sdr_signal_processing.cc b/src/algorithms/libs/gps_sdr_signal_processing.cc index 945be5607..3375e3b40 100644 --- a/src/algorithms/libs/gps_sdr_signal_processing.cc +++ b/src/algorithms/libs/gps_sdr_signal_processing.cc @@ -34,7 +34,7 @@ auto auxCeil = [](float x) { return static_cast(static_cast((x) + 1)); }; -void gps_l1_ca_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift) +void gps_l1_ca_code_gen_int(gsl::span _dest, int32_t _prn, uint32_t _chip_shift) { const uint32_t _code_length = 1023; bool G1[_code_length]; @@ -116,7 +116,7 @@ void gps_l1_ca_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift) } -void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift) +void gps_l1_ca_code_gen_float(gsl::span _dest, int32_t _prn, uint32_t _chip_shift) { const uint32_t _code_length = 1023; int32_t ca_code_int[_code_length]; @@ -130,7 +130,7 @@ void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift) } -void gps_l1_ca_code_gen_complex(std::complex* _dest, int32_t _prn, uint32_t _chip_shift) +void gps_l1_ca_code_gen_complex(gsl::span> _dest, int32_t _prn, uint32_t _chip_shift) { const uint32_t _code_length = 1023; int32_t ca_code_int[_code_length] = {0}; @@ -148,7 +148,7 @@ void gps_l1_ca_code_gen_complex(std::complex* _dest, int32_t _prn, uint32 * Generates complex GPS L1 C/A code for the desired SV ID and sampled to specific sampling frequency * NOTICE: the number of samples is rounded towards zero (integer truncation) */ -void gps_l1_ca_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) +void gps_l1_ca_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { // This function is based on the GNU software GPS for MATLAB in the Kay Borre book std::complex _code[1023]; diff --git a/src/algorithms/libs/gps_sdr_signal_processing.h b/src/algorithms/libs/gps_sdr_signal_processing.h index 44bb815dd..27a5f7eee 100644 --- a/src/algorithms/libs/gps_sdr_signal_processing.h +++ b/src/algorithms/libs/gps_sdr_signal_processing.h @@ -36,19 +36,26 @@ #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif + //! Generates int GPS L1 C/A code for the desired SV ID and code shift -void gps_l1_ca_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift); +void gps_l1_ca_code_gen_int(gsl::span _dest, int32_t _prn, uint32_t _chip_shift); //! Generates float GPS L1 C/A code for the desired SV ID and code shift -void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift); +void gps_l1_ca_code_gen_float(gsl::span _dest, int32_t _prn, uint32_t _chip_shift); //! Generates complex GPS L1 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency -void gps_l1_ca_code_gen_complex(std::complex* _dest, int32_t _prn, uint32_t _chip_shift); +void gps_l1_ca_code_gen_complex(gsl::span> _dest, int32_t _prn, uint32_t _chip_shift); //! Generates N complex GPS L1 C/A codes for the desired SV ID and code shift -void gps_l1_ca_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); +void gps_l1_ca_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, uint32_t _ncodes); //! Generates complex GPS L1 C/A code for the desired SV ID and code shift -void gps_l1_ca_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); +void gps_l1_ca_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GPS_SDR_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/gsl/include/gsl/gsl b/src/algorithms/libs/gsl/include/gsl/gsl new file mode 100644 index 000000000..55862ebdd --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/gsl @@ -0,0 +1,29 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_GSL_H +#define GSL_GSL_H + +#include // copy +#include // Ensures/Expects +#include // byte +#include // finally()/narrow()/narrow_cast()... +#include // multi_span, strided_span... +#include // owner, not_null +#include // span +#include // zstring, string_span, zstring_builder... + +#endif // GSL_GSL_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_algorithm b/src/algorithms/libs/gsl/include/gsl/gsl_algorithm new file mode 100644 index 000000000..c2ba31f30 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/gsl_algorithm @@ -0,0 +1,61 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_ALGORITHM_H +#define GSL_ALGORITHM_H + +#include // for Expects +#include // for dynamic_extent, span + +#include // for copy_n +#include // for ptrdiff_t +#include // for is_assignable + +#ifdef _MSC_VER +#pragma warning(push) + +// turn off some warnings that are noisy about our Expects statements +#pragma warning(disable : 4127) // conditional expression is constant +#pragma warning(disable : 4996) // unsafe use of std::copy_n + +#endif // _MSC_VER + +namespace gsl +{ +// Note: this will generate faster code than std::copy using span iterator in older msvc+stl +// not necessary for msvc since VS2017 15.8 (_MSC_VER >= 1915) +template +void copy(span src, span dest) +{ + static_assert(std::is_assignable::value, + "Elements of source span can not be assigned to elements of destination span"); + static_assert(SrcExtent == dynamic_extent || DestExtent == dynamic_extent || + (SrcExtent <= DestExtent), + "Source range is longer than target range"); + + Expects(dest.size() >= src.size()); + GSL_SUPPRESS(stl.1) // NO-FORMAT: attribute + std::copy_n(src.data(), src.size(), dest.data()); +} + +} // namespace gsl + +#ifdef _MSC_VER +#pragma warning(pop) +#endif // _MSC_VER + +#endif // GSL_ALGORITHM_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_assert b/src/algorithms/libs/gsl/include/gsl/gsl_assert new file mode 100644 index 000000000..c70463395 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/gsl_assert @@ -0,0 +1,177 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_CONTRACTS_H +#define GSL_CONTRACTS_H + +#include +#include // for logic_error + +// +// make suppress attributes parse for some compilers +// Hopefully temporary until suppression standardization occurs +// +#if defined(__clang__) +#define GSL_SUPPRESS(x) [[gsl::suppress("x")]] +#else +#if defined(_MSC_VER) +#define GSL_SUPPRESS(x) [[gsl::suppress(x)]] +#else +#define GSL_SUPPRESS(x) +#endif // _MSC_VER +#endif // __clang__ + +// +// Temporary until MSVC STL supports no-exceptions mode. +// Currently terminate is a no-op in this mode, so we add termination behavior back +// +#if defined(_MSC_VER) && defined(_HAS_EXCEPTIONS) && !_HAS_EXCEPTIONS +#define GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND +#include +#define RANGE_CHECKS_FAILURE 0 + +#if defined(__clang__) +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Winvalid-noreturn" +#endif + +#endif + +// +// There are three configuration options for this GSL implementation's behavior +// when pre/post conditions on the GSL types are violated: +// +// 1. GSL_TERMINATE_ON_CONTRACT_VIOLATION: std::terminate will be called (default) +// 2. GSL_THROW_ON_CONTRACT_VIOLATION: a gsl::fail_fast exception will be thrown +// 3. GSL_UNENFORCED_ON_CONTRACT_VIOLATION: nothing happens +// +#if !(defined(GSL_THROW_ON_CONTRACT_VIOLATION) || defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) || \ + defined(GSL_UNENFORCED_ON_CONTRACT_VIOLATION)) +#define GSL_TERMINATE_ON_CONTRACT_VIOLATION +#endif + +#define GSL_STRINGIFY_DETAIL(x) #x +#define GSL_STRINGIFY(x) GSL_STRINGIFY_DETAIL(x) + +#if defined(__clang__) || defined(__GNUC__) +#define GSL_LIKELY(x) __builtin_expect(!!(x), 1) +#define GSL_UNLIKELY(x) __builtin_expect(!!(x), 0) +#else +#define GSL_LIKELY(x) (!!(x)) +#define GSL_UNLIKELY(x) (!!(x)) +#endif + +// +// GSL_ASSUME(cond) +// +// Tell the optimizer that the predicate cond must hold. It is unspecified +// whether or not cond is actually evaluated. +// +#ifdef _MSC_VER +#define GSL_ASSUME(cond) __assume(cond) +#elif defined(__GNUC__) +#define GSL_ASSUME(cond) ((cond) ? static_cast(0) : __builtin_unreachable()) +#else +#define GSL_ASSUME(cond) static_cast((cond) ? 0 : 0) +#endif + +// +// GSL.assert: assertions +// + +namespace gsl +{ +struct fail_fast : public std::logic_error +{ + explicit fail_fast(char const* const message) : std::logic_error(message) {} +}; + +namespace details +{ +#if defined(GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND) + + typedef void (__cdecl *terminate_handler)(); + + GSL_SUPPRESS(f.6) // NO-FORMAT: attribute + [[noreturn]] inline void __cdecl default_terminate_handler() + { + __fastfail(RANGE_CHECKS_FAILURE); + } + + inline gsl::details::terminate_handler& get_terminate_handler() noexcept + { + static terminate_handler handler = &default_terminate_handler; + return handler; + } + +#endif + + [[noreturn]] inline void terminate() noexcept + { +#if defined(GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND) + (*gsl::details::get_terminate_handler())(); +#else + std::terminate(); +#endif + } + +#if defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) + + template + [[noreturn]] void throw_exception(Exception&&) noexcept + { + gsl::details::terminate(); + } + +#else + + template + [[noreturn]] void throw_exception(Exception&& exception) + { + throw std::forward(exception); + } + +#endif // GSL_TERMINATE_ON_CONTRACT_VIOLATION + +} // namespace details +} // namespace gsl + +#if defined(GSL_THROW_ON_CONTRACT_VIOLATION) + +#define GSL_CONTRACT_CHECK(type, cond) \ + (GSL_LIKELY(cond) ? static_cast(0) \ + : gsl::details::throw_exception(gsl::fail_fast( \ + "GSL: " type " failure at " __FILE__ ": " GSL_STRINGIFY(__LINE__)))) + +#elif defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) + +#define GSL_CONTRACT_CHECK(type, cond) \ + (GSL_LIKELY(cond) ? static_cast(0) : gsl::details::terminate()) + +#elif defined(GSL_UNENFORCED_ON_CONTRACT_VIOLATION) + +#define GSL_CONTRACT_CHECK(type, cond) GSL_ASSUME(cond) + +#endif // GSL_THROW_ON_CONTRACT_VIOLATION + +#define Expects(cond) GSL_CONTRACT_CHECK("Precondition", cond) +#define Ensures(cond) GSL_CONTRACT_CHECK("Postcondition", cond) + +#if defined(GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND) && defined(__clang__) +#pragma clang diagnostic pop +#endif + +#endif // GSL_CONTRACTS_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_byte b/src/algorithms/libs/gsl/include/gsl/gsl_byte new file mode 100644 index 000000000..861446dc5 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/gsl_byte @@ -0,0 +1,203 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_BYTE_H +#define GSL_BYTE_H + +// +// make suppress attributes work for some compilers +// Hopefully temporary until suppression standardization occurs +// +#if defined(__clang__) +#define GSL_SUPPRESS(x) [[gsl::suppress("x")]] +#else +#if defined(_MSC_VER) +#define GSL_SUPPRESS(x) [[gsl::suppress(x)]] +#else +#define GSL_SUPPRESS(x) +#endif // _MSC_VER +#endif // __clang__ + +#include + +#ifdef _MSC_VER + +#pragma warning(push) + +// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. +#pragma warning(disable : 26493) // don't use c-style casts // TODO: MSVC suppression in templates does not always work + +#ifndef GSL_USE_STD_BYTE +// this tests if we are under MSVC and the standard lib has std::byte and it is enabled +#if defined(_HAS_STD_BYTE) && _HAS_STD_BYTE + +#define GSL_USE_STD_BYTE 1 + +#else // defined(_HAS_STD_BYTE) && _HAS_STD_BYTE + +#define GSL_USE_STD_BYTE 0 + +#endif // defined(_HAS_STD_BYTE) && _HAS_STD_BYTE +#endif // GSL_USE_STD_BYTE + +#else // _MSC_VER + +#ifndef GSL_USE_STD_BYTE +// this tests if we are under GCC or Clang with enough -std:c++1z power to get us std::byte +// also check if libc++ version is sufficient (> 5.0) or libstc++ actually contains std::byte +#if defined(__cplusplus) && (__cplusplus >= 201703L) && \ + (defined(__cpp_lib_byte) && (__cpp_lib_byte >= 201603) || \ + defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 5000)) + +#define GSL_USE_STD_BYTE 1 +#include + +#else // defined(__cplusplus) && (__cplusplus >= 201703L) && + // (defined(__cpp_lib_byte) && (__cpp_lib_byte >= 201603) || + // defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 5000)) + +#define GSL_USE_STD_BYTE 0 + +#endif //defined(__cplusplus) && (__cplusplus >= 201703L) && + // (defined(__cpp_lib_byte) && (__cpp_lib_byte >= 201603) || + // defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 5000)) +#endif // GSL_USE_STD_BYTE + +#endif // _MSC_VER + +// Use __may_alias__ attribute on gcc and clang +#if defined __clang__ || (defined(__GNUC__) && __GNUC__ > 5) +#define byte_may_alias __attribute__((__may_alias__)) +#else // defined __clang__ || defined __GNUC__ +#define byte_may_alias +#endif // defined __clang__ || defined __GNUC__ + +namespace gsl +{ +#if GSL_USE_STD_BYTE + +using std::byte; +using std::to_integer; + +#else // GSL_USE_STD_BYTE + +// This is a simple definition for now that allows +// use of byte within span<> to be standards-compliant +enum class byte_may_alias byte : unsigned char +{ +}; + +template ::value>> +constexpr byte& operator<<=(byte& b, IntegerType shift) noexcept +{ + return b = byte(static_cast(b) << shift); +} + +template ::value>> +constexpr byte operator<<(byte b, IntegerType shift) noexcept +{ + return byte(static_cast(b) << shift); +} + +template ::value>> +constexpr byte& operator>>=(byte& b, IntegerType shift) noexcept +{ + return b = byte(static_cast(b) >> shift); +} + +template ::value>> +constexpr byte operator>>(byte b, IntegerType shift) noexcept +{ + return byte(static_cast(b) >> shift); +} + +constexpr byte& operator|=(byte& l, byte r) noexcept +{ + return l = byte(static_cast(l) | static_cast(r)); +} + +constexpr byte operator|(byte l, byte r) noexcept +{ + return byte(static_cast(l) | static_cast(r)); +} + +constexpr byte& operator&=(byte& l, byte r) noexcept +{ + return l = byte(static_cast(l) & static_cast(r)); +} + +constexpr byte operator&(byte l, byte r) noexcept +{ + return byte(static_cast(l) & static_cast(r)); +} + +constexpr byte& operator^=(byte& l, byte r) noexcept +{ + return l = byte(static_cast(l) ^ static_cast(r)); +} + +constexpr byte operator^(byte l, byte r) noexcept +{ + return byte(static_cast(l) ^ static_cast(r)); +} + +constexpr byte operator~(byte b) noexcept { return byte(~static_cast(b)); } + +template ::value>> +constexpr IntegerType to_integer(byte b) noexcept +{ + return static_cast(b); +} + +#endif // GSL_USE_STD_BYTE + +template +constexpr byte to_byte_impl(T t) noexcept +{ + static_assert( + E, "gsl::to_byte(t) must be provided an unsigned char, otherwise data loss may occur. " + "If you are calling to_byte with an integer contant use: gsl::to_byte() version."); + return static_cast(t); +} +template <> +// NOTE: need suppression since c++14 does not allow "return {t}" +// GSL_SUPPRESS(type.4) // NO-FORMAT: attribute // TODO: suppression does not work +constexpr byte to_byte_impl(unsigned char t) noexcept +{ + return byte(t); +} + +template +constexpr byte to_byte(T t) noexcept +{ + return to_byte_impl::value, T>(t); +} + +template +constexpr byte to_byte() noexcept +{ + static_assert(I >= 0 && I <= 255, + "gsl::byte only has 8 bits of storage, values must be in range 0-255"); + return static_cast(I); +} + +} // namespace gsl + +#ifdef _MSC_VER +#pragma warning(pop) +#endif // _MSC_VER + +#endif // GSL_BYTE_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_util b/src/algorithms/libs/gsl/include/gsl/gsl_util new file mode 100644 index 000000000..4addde6ad --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/gsl_util @@ -0,0 +1,175 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_UTIL_H +#define GSL_UTIL_H + +#include // for Expects + +#include +#include // for ptrdiff_t, size_t +#include // for exception +#include // for initializer_list +#include // for is_signed, integral_constant +#include // for forward + +#if defined(_MSC_VER) && !defined(__clang__) + +#pragma warning(push) +#pragma warning(disable : 4127) // conditional expression is constant + +#if _MSC_VER < 1910 +#pragma push_macro("constexpr") +#define constexpr /*constexpr*/ +#endif // _MSC_VER < 1910 +#endif // _MSC_VER + +#if (defined(_MSC_VER) && _MSC_VER < 1910) || (!defined(__clang__) && defined(__GNUC__) && __GUNC__ < 6) +#define GSL_CONSTEXPR_NARROW 0 +#else +#define GSL_CONSTEXPR_NARROW 1 +#endif + +namespace gsl +{ +// +// GSL.util: utilities +// + +// index type for all container indexes/subscripts/sizes +using index = std::ptrdiff_t; + +// final_action allows you to ensure something gets run at the end of a scope +template +class final_action +{ +public: + explicit final_action(F f) noexcept : f_(std::move(f)) {} + + final_action(final_action&& other) noexcept : f_(std::move(other.f_)), invoke_(other.invoke_) + { + other.invoke_ = false; + } + + final_action(const final_action&) = delete; + final_action& operator=(const final_action&) = delete; + final_action& operator=(final_action&&) = delete; + + GSL_SUPPRESS(f.6) // NO-FORMAT: attribute // terminate if throws + ~final_action() noexcept + { + if (invoke_) f_(); + } + +private: + F f_; + bool invoke_{true}; +}; + +// finally() - convenience function to generate a final_action +template +final_action finally(const F& f) noexcept +{ + return final_action(f); +} + +template +final_action finally(F&& f) noexcept +{ + return final_action(std::forward(f)); +} + +// narrow_cast(): a searchable way to do narrowing casts of values +template +GSL_SUPPRESS(type.1) // NO-FORMAT: attribute +constexpr T narrow_cast(U&& u) noexcept +{ + return static_cast(std::forward(u)); +} + +struct narrowing_error : public std::exception +{ +}; + +namespace details +{ + template + struct is_same_signedness + : public std::integral_constant::value == std::is_signed::value> + { + }; +} // namespace details + +// narrow() : a checked version of narrow_cast() that throws if the cast changed the value +template +GSL_SUPPRESS(type.1) // NO-FORMAT: attribute +GSL_SUPPRESS(f.6) // NO-FORMAT: attribute // TODO: MSVC /analyze does not recognise noexcept(false) +#if GSL_CONSTEXPR_NARROW +constexpr +#endif +T narrow(U u) noexcept(false) +{ + T t = narrow_cast(u); + if (static_cast(t) != u) gsl::details::throw_exception(narrowing_error()); + if (!details::is_same_signedness::value && ((t < T{}) != (u < U{}))) + gsl::details::throw_exception(narrowing_error()); + return t; +} + +// +// at() - Bounds-checked way of accessing builtin arrays, std::array, std::vector +// +template +GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute +GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute +constexpr T& at(T (&arr)[N], const index i) +{ + Expects(i >= 0 && i < narrow_cast(N)); + return arr[narrow_cast(i)]; +} + +template +GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute +GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute +constexpr auto at(Cont& cont, const index i) -> decltype(cont[cont.size()]) +{ + Expects(i >= 0 && i < narrow_cast(cont.size())); + using size_type = decltype(cont.size()); + return cont[narrow_cast(i)]; +} + +template +GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute +constexpr T at(const std::initializer_list cont, const index i) +{ + Expects(i >= 0 && i < narrow_cast(cont.size())); + return *(cont.begin() + i); +} + +} // namespace gsl + +#if defined(_MSC_VER) && !defined(__clang__) +#if _MSC_VER < 1910 +#undef constexpr +#pragma pop_macro("constexpr") + +#endif // _MSC_VER < 1910 + +#pragma warning(pop) + +#endif // _MSC_VER + +#endif // GSL_UTIL_H diff --git a/src/algorithms/libs/gsl/include/gsl/multi_span b/src/algorithms/libs/gsl/include/gsl/multi_span new file mode 100644 index 000000000..0c1506ed4 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/multi_span @@ -0,0 +1,2293 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_MULTI_SPAN_H +#define GSL_MULTI_SPAN_H + +#include // for Expects +#include // for byte +#include // for narrow_cast + +#include // for transform, lexicographical_compare +#include // for array +#include +#include // for ptrdiff_t, size_t, nullptr_t +#include // for PTRDIFF_MAX +#include // for divides, multiplies, minus, negate, plus +#include // for initializer_list +#include // for iterator, random_access_iterator_tag +#include // for numeric_limits +#include +#include +#include +#include // for basic_string +#include // for enable_if_t, remove_cv_t, is_same, is_co... +#include + +#if defined(_MSC_VER) && !defined(__clang__) + +// turn off some warnings that are noisy about our Expects statements +#pragma warning(push) +#pragma warning(disable : 4127) // conditional expression is constant +#pragma warning(disable : 4702) // unreachable code + +// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. +#pragma warning(disable : 26495) // uninitalized member when constructor calls constructor +#pragma warning(disable : 26473) // in some instantiations we cast to the same type +#pragma warning(disable : 26490) // TODO: bug in parser - attributes and templates +#pragma warning(disable : 26465) // TODO: bug - suppression does not work on template functions + +#if _MSC_VER < 1910 +#pragma push_macro("constexpr") +#define constexpr /*constexpr*/ + +#endif // _MSC_VER < 1910 +#endif // _MSC_VER + +// GCC 7 does not like the signed unsigned missmatch (size_t ptrdiff_t) +// While there is a conversion from signed to unsigned, it happens at +// compiletime, so the compiler wouldn't have to warn indiscriminently, but +// could check if the source value actually doesn't fit into the target type +// and only warn in those cases. +#if defined(__GNUC__) && __GNUC__ > 6 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wsign-conversion" +#endif + +namespace gsl +{ + +/* +** begin definitions of index and bounds +*/ +namespace details +{ + template + struct SizeTypeTraits + { + static const SizeType max_value = std::numeric_limits::max(); + }; + + template + class are_integral : public std::integral_constant + { + }; + + template + class are_integral + : public std::integral_constant::value && are_integral::value> + { + }; +} // namespace details + +template +class multi_span_index final +{ + static_assert(Rank > 0, "Rank must be greater than 0!"); + + template + friend class multi_span_index; + +public: + static const std::size_t rank = Rank; + using value_type = std::ptrdiff_t; + using size_type = value_type; + using reference = std::add_lvalue_reference_t; + using const_reference = std::add_lvalue_reference_t>; + + constexpr multi_span_index() noexcept {} + + constexpr multi_span_index(const value_type (&values)[Rank]) noexcept + { + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute + std::copy(values, values + Rank, elems); + } + + template ::value>> + constexpr multi_span_index(Ts... ds) noexcept : elems{narrow_cast(ds)...} + {} + + constexpr multi_span_index(const multi_span_index& other) noexcept = default; + + constexpr multi_span_index& operator=(const multi_span_index& rhs) noexcept = default; + + // Preconditions: component_idx < rank + GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr reference operator[](std::size_t component_idx) + { + Expects(component_idx < Rank); // Component index must be less than rank + return elems[component_idx]; + } + + // Preconditions: component_idx < rank + GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr const_reference operator[](std::size_t component_idx) const + { + Expects(component_idx < Rank); // Component index must be less than rank + return elems[component_idx]; + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute + constexpr bool operator==(const multi_span_index& rhs) const + { + return std::equal(elems, elems + rank, rhs.elems); + } + + constexpr bool operator!=(const multi_span_index& rhs) const + { + return !(*this == rhs); + } + + constexpr multi_span_index operator+() const noexcept { return *this; } + + constexpr multi_span_index operator-() const + { + multi_span_index ret = *this; + std::transform(ret, ret + rank, ret, std::negate{}); + return ret; + } + + constexpr multi_span_index operator+(const multi_span_index& rhs) const + { + multi_span_index ret = *this; + ret += rhs; + return ret; + } + + constexpr multi_span_index operator-(const multi_span_index& rhs) const + { + multi_span_index ret = *this; + ret -= rhs; + return ret; + } + + constexpr multi_span_index& operator+=(const multi_span_index& rhs) + { + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute + std::transform(elems, elems + rank, rhs.elems, elems, + std::plus{}); + return *this; + } + + constexpr multi_span_index& operator-=(const multi_span_index& rhs) + { + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute + std::transform(elems, elems + rank, rhs.elems, elems, std::minus{}); + return *this; + } + + constexpr multi_span_index operator*(value_type v) const + { + multi_span_index ret = *this; + ret *= v; + return ret; + } + + constexpr multi_span_index operator/(value_type v) const + { + multi_span_index ret = *this; + ret /= v; + return ret; + } + + friend constexpr multi_span_index operator*(value_type v, const multi_span_index& rhs) + { + return rhs * v; + } + + constexpr multi_span_index& operator*=(value_type v) + { + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute + std::transform(elems, elems + rank, elems, + [v](value_type x) { return std::multiplies{}(x, v); }); + return *this; + } + + constexpr multi_span_index& operator/=(value_type v) + { + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute + std::transform(elems, elems + rank, elems, + [v](value_type x) { return std::divides{}(x, v); }); + return *this; + } + +private: + value_type elems[Rank] = {}; +}; + +#if !defined(_MSC_VER) || _MSC_VER >= 1910 + +struct static_bounds_dynamic_range_t +{ + template ::value>> + constexpr operator T() const noexcept + { + return narrow_cast(-1); + } +}; + +constexpr bool operator==(static_bounds_dynamic_range_t, static_bounds_dynamic_range_t) noexcept +{ + return true; +} + +constexpr bool operator!=(static_bounds_dynamic_range_t, static_bounds_dynamic_range_t) noexcept +{ + return false; +} + +template ::value>> +constexpr bool operator==(static_bounds_dynamic_range_t, T other) noexcept +{ + return narrow_cast(-1) == other; +} + +template ::value>> +constexpr bool operator==(T left, static_bounds_dynamic_range_t right) noexcept +{ + return right == left; +} + +template ::value>> +constexpr bool operator!=(static_bounds_dynamic_range_t, T other) noexcept +{ + return narrow_cast(-1) != other; +} + +template ::value>> +constexpr bool operator!=(T left, static_bounds_dynamic_range_t right) noexcept +{ + return right != left; +} + +constexpr static_bounds_dynamic_range_t dynamic_range{}; +#else +const std::ptrdiff_t dynamic_range = -1; +#endif + +struct generalized_mapping_tag +{ +}; +struct contiguous_mapping_tag : generalized_mapping_tag +{ +}; + +namespace details +{ + + template + struct LessThan + { + static const bool value = Left < Right; + }; + + template + struct BoundsRanges + { + using size_type = std::ptrdiff_t; + static const size_type Depth = 0; + static const size_type DynamicNum = 0; + static const size_type CurrentRange = 1; + static const size_type TotalSize = 1; + + // TODO : following signature is for work around VS bug + template + constexpr BoundsRanges(const OtherRange&, bool /* firstLevel */) + {} + + constexpr BoundsRanges(const std::ptrdiff_t* const) {} + constexpr BoundsRanges() noexcept = default; + + template + constexpr void serialize(T&) const + {} + + template + constexpr size_type linearize(const T&) const + { + return 0; + } + + template + constexpr size_type contains(const T&) const + { + return -1; + } + + constexpr size_type elementNum(std::size_t) const noexcept { return 0; } + + constexpr size_type totalSize() const noexcept { return TotalSize; } + + constexpr bool operator==(const BoundsRanges&) const noexcept { return true; } + }; + + template + struct BoundsRanges : BoundsRanges + { + using Base = BoundsRanges; + using size_type = std::ptrdiff_t; + static const std::size_t Depth = Base::Depth + 1; + static const std::size_t DynamicNum = Base::DynamicNum + 1; + static const size_type CurrentRange = dynamic_range; + static const size_type TotalSize = dynamic_range; + + private: + size_type m_bound; + + public: + GSL_SUPPRESS(f.23) // NO-FORMAT: attribute // this pointer type is cannot be assigned nullptr - issue in not_null + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr BoundsRanges(const std::ptrdiff_t* const arr) + : Base(arr + 1), m_bound(*arr * this->Base::totalSize()) + { + Expects(0 <= *arr); + } + + constexpr BoundsRanges() noexcept : m_bound(0) {} + + template + constexpr BoundsRanges(const BoundsRanges& other, + bool /* firstLevel */ = true) + : Base(static_cast&>(other), false) + , m_bound(other.totalSize()) + {} + + template + constexpr void serialize(T& arr) const + { + arr[Dim] = elementNum(); + this->Base::template serialize(arr); + } + + template + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr size_type linearize(const T& arr) const + { + const size_type index = this->Base::totalSize() * arr[Dim]; + Expects(index < m_bound); + return index + this->Base::template linearize(arr); + } + + template + constexpr size_type contains(const T& arr) const + { + const ptrdiff_t last = this->Base::template contains(arr); + if (last == -1) return -1; + const ptrdiff_t cur = this->Base::totalSize() * arr[Dim]; + return cur < m_bound ? cur + last : -1; + } + + GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used + constexpr size_type totalSize() const noexcept + { + return m_bound; + } + + GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used + constexpr size_type elementNum() const noexcept + { + return totalSize() / this->Base::totalSize(); + } + + GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used + constexpr size_type elementNum(std::size_t dim) const noexcept + { + if (dim > 0) + return this->Base::elementNum(dim - 1); + else + return elementNum(); + } + + constexpr bool operator==(const BoundsRanges& rhs) const noexcept + { + return m_bound == rhs.m_bound && + static_cast(*this) == static_cast(rhs); + } + }; + + template + struct BoundsRanges : BoundsRanges + { + using Base = BoundsRanges; + using size_type = std::ptrdiff_t; + static const std::size_t Depth = Base::Depth + 1; + static const std::size_t DynamicNum = Base::DynamicNum; + static const size_type CurrentRange = CurRange; + static const size_type TotalSize = + Base::TotalSize == dynamic_range ? dynamic_range : CurrentRange * Base::TotalSize; + + constexpr BoundsRanges(const std::ptrdiff_t* const arr) : Base(arr) {} + constexpr BoundsRanges() = default; + + template + constexpr BoundsRanges(const BoundsRanges& other, + bool firstLevel = true) + : Base(static_cast&>(other), false) + { + GSL_SUPPRESS(type.4) // NO-FORMAT: attribute // TODO: false positive + (void) firstLevel; + } + + template + constexpr void serialize(T& arr) const + { + arr[Dim] = elementNum(); + this->Base::template serialize(arr); + } + + template + constexpr size_type linearize(const T& arr) const + { + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + Expects(arr[Dim] >= 0 && arr[Dim] < CurrentRange); // Index is out of range + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + const ptrdiff_t d = arr[Dim]; + return this->Base::totalSize() * d + + this->Base::template linearize(arr); + } + + template + constexpr size_type contains(const T& arr) const + { + if (arr[Dim] >= CurrentRange) return -1; + const size_type last = this->Base::template contains(arr); + if (last == -1) return -1; + return this->Base::totalSize() * arr[Dim] + last; + } + + GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used + constexpr size_type totalSize() const noexcept + { + return CurrentRange * this->Base::totalSize(); + } + + GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used + constexpr size_type elementNum() const noexcept + { + return CurrentRange; + } + + GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used + constexpr size_type elementNum(std::size_t dim) const noexcept + { + if (dim > 0) + return this->Base::elementNum(dim - 1); + else + return elementNum(); + } + + constexpr bool operator==(const BoundsRanges& rhs) const noexcept + { + return static_cast(*this) == static_cast(rhs); + } + }; + + template + struct BoundsRangeConvertible + : public std::integral_constant= TargetType::TotalSize || + TargetType::TotalSize == dynamic_range || + SourceType::TotalSize == dynamic_range || + TargetType::TotalSize == 0)> + { + }; + + template + struct TypeListIndexer + { + const TypeChain& obj_; + constexpr TypeListIndexer(const TypeChain& obj) : obj_(obj) {} + + template + constexpr const TypeChain& getObj(std::true_type) + { + return obj_; + } + + template + constexpr auto getObj(std::false_type) + -> decltype(TypeListIndexer(static_cast(obj_)).template get()) + { + return TypeListIndexer(static_cast(obj_)).template get(); + } + + template + constexpr auto get() -> decltype(getObj(std::integral_constant())) + { + return getObj(std::integral_constant()); + } + }; + + template + constexpr TypeListIndexer createTypeListIndexer(const TypeChain& obj) + { + return TypeListIndexer(obj); + } + + template 1), + typename Ret = std::enable_if_t>> + constexpr Ret shift_left(const multi_span_index& other) noexcept + { + Ret ret{}; + for (std::size_t i = 0; i < Rank - 1; ++i) + { + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + ret[i] = other[i + 1]; + } + return ret; + } +} // namespace details + +template +class bounds_iterator; + +template +class static_bounds +{ +public: + static_bounds(const details::BoundsRanges&) {} +}; + +template +class static_bounds +{ + using MyRanges = details::BoundsRanges; + + MyRanges m_ranges; + constexpr static_bounds(const MyRanges& range) noexcept : m_ranges(range) {} + + template + friend class static_bounds; + +public: + static const std::size_t rank = MyRanges::Depth; + static const std::size_t dynamic_rank = MyRanges::DynamicNum; + static const std::ptrdiff_t static_size = MyRanges::TotalSize; + + using size_type = std::ptrdiff_t; + using index_type = multi_span_index; + using const_index_type = std::add_const_t; + using iterator = bounds_iterator; + using const_iterator = bounds_iterator; + using difference_type = std::ptrdiff_t; + using sliced_type = static_bounds; + using mapping_type = contiguous_mapping_tag; + + constexpr static_bounds() /*noexcept*/ = default; + + template + struct BoundsRangeConvertible2; + + template > + static auto helpBoundsRangeConvertible(SourceType, TargetType, std::true_type) -> Ret; + + template + static auto helpBoundsRangeConvertible(SourceType, TargetType, ...) -> std::false_type; + + template + struct BoundsRangeConvertible2 + : decltype(helpBoundsRangeConvertible( + SourceType(), TargetType(), + std::integral_constant())) + { + }; + + template + struct BoundsRangeConvertible2 : std::true_type + { + }; + + template + struct BoundsRangeConvertible + : decltype(helpBoundsRangeConvertible( + SourceType(), TargetType(), + std::integral_constant::value || + TargetType::CurrentRange == dynamic_range || + SourceType::CurrentRange == dynamic_range)>())) + { + }; + + template + struct BoundsRangeConvertible : std::true_type + { + }; + + template , + details::BoundsRanges>::value>> + constexpr static_bounds(const static_bounds& other) : m_ranges(other.m_ranges) + { + Expects((MyRanges::DynamicNum == 0 && details::BoundsRanges::DynamicNum == 0) || + MyRanges::DynamicNum > 0 || other.m_ranges.totalSize() >= m_ranges.totalSize()); + } + + constexpr static_bounds(std::initializer_list il) : m_ranges(il.begin()) + { + // Size of the initializer list must match the rank of the array + Expects((MyRanges::DynamicNum == 0 && il.size() == 1 && *il.begin() == static_size) || + MyRanges::DynamicNum == il.size()); + // Size of the range must be less than the max element of the size type + Expects(m_ranges.totalSize() <= PTRDIFF_MAX); + } + + constexpr sliced_type slice() const noexcept + { + return sliced_type{static_cast&>(m_ranges)}; + } + + constexpr size_type stride() const noexcept { return rank > 1 ? slice().size() : 1; } + + constexpr size_type size() const noexcept { return m_ranges.totalSize(); } + + constexpr size_type total_size() const noexcept { return m_ranges.totalSize(); } + + constexpr size_type linearize(const index_type& idx) const { return m_ranges.linearize(idx); } + + constexpr bool contains(const index_type& idx) const noexcept + { + return m_ranges.contains(idx) != -1; + } + + constexpr size_type operator[](std::size_t idx) const noexcept + { + return m_ranges.elementNum(idx); + } + + template + constexpr size_type extent() const noexcept + { + static_assert(Dim < rank, + "dimension should be less than rank (dimension count starts from 0)"); + return details::createTypeListIndexer(m_ranges).template get().elementNum(); + } + + template + constexpr size_type extent(IntType dim) const + { + static_assert(std::is_integral::value, + "Dimension parameter must be supplied as an integral type."); + auto real_dim = narrow_cast(dim); + Expects(real_dim < rank); + + return m_ranges.elementNum(real_dim); + } + + constexpr index_type index_bounds() const noexcept + { + size_type extents[rank] = {}; + m_ranges.serialize(extents); + return {extents}; + } + + template + constexpr bool operator==(const static_bounds& rhs) const noexcept + { + return this->size() == rhs.size(); + } + + template + constexpr bool operator!=(const static_bounds& rhs) const noexcept + { + return !(*this == rhs); + } + + constexpr const_iterator begin() const noexcept + { + return const_iterator(*this, index_type{}); + } + + constexpr const_iterator end() const noexcept + { + return const_iterator(*this, this->index_bounds()); + } +}; + +template +class strided_bounds { + template + friend class strided_bounds; + +public: + static const std::size_t rank = Rank; + using value_type = std::ptrdiff_t; + using reference = std::add_lvalue_reference_t; + using const_reference = std::add_const_t; + using size_type = value_type; + using difference_type = value_type; + using index_type = multi_span_index; + using const_index_type = std::add_const_t; + using iterator = bounds_iterator; + using const_iterator = bounds_iterator; + static const value_type dynamic_rank = rank; + static const value_type static_size = dynamic_range; + using sliced_type = std::conditional_t, void>; + using mapping_type = generalized_mapping_tag; + + constexpr strided_bounds(const strided_bounds&) noexcept = default; + + constexpr strided_bounds& operator=(const strided_bounds&) noexcept = default; + + constexpr strided_bounds(const value_type (&values)[rank], index_type strides) + : m_extents(values), m_strides(std::move(strides)) + {} + + constexpr strided_bounds(const index_type& extents, const index_type& strides) noexcept + : m_extents(extents), m_strides(strides) + { + } + + constexpr index_type strides() const noexcept { return m_strides; } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr size_type total_size() const noexcept + { + size_type ret = 0; + for (std::size_t i = 0; i < rank; ++i) { ret += (m_extents[i] - 1) * m_strides[i]; } + return ret + 1; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr size_type size() const noexcept + { + size_type ret = 1; + for (std::size_t i = 0; i < rank; ++i) { ret *= m_extents[i]; } + return ret; + } + + constexpr bool contains(const index_type& idx) const noexcept + { + for (std::size_t i = 0; i < rank; ++i) + { + if (idx[i] < 0 || idx[i] >= m_extents[i]) return false; + } + return true; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr size_type linearize(const index_type& idx) const + { + size_type ret = 0; + for (std::size_t i = 0; i < rank; i++) + { + Expects(idx[i] < m_extents[i]); // index is out of bounds of the array + ret += idx[i] * m_strides[i]; + } + return ret; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr size_type stride() const noexcept { return m_strides[0]; } + + template 1), typename Ret = std::enable_if_t> + constexpr sliced_type slice() const + { + return {details::shift_left(m_extents), details::shift_left(m_strides)}; + } + + template + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr size_type extent() const noexcept + { + static_assert(Dim < Rank, + "dimension should be less than rank (dimension count starts from 0)"); + return m_extents[Dim]; + } + + constexpr index_type index_bounds() const noexcept { return m_extents; } + + constexpr const_iterator begin() const noexcept { return const_iterator{*this, index_type{}}; } + + constexpr const_iterator end() const noexcept { return const_iterator{*this, index_bounds()}; } + +private: + index_type m_extents; + index_type m_strides; +}; + +template +struct is_bounds : std::integral_constant +{ +}; +template +struct is_bounds> : std::integral_constant +{ +}; +template +struct is_bounds> : std::integral_constant +{ +}; + +template +class bounds_iterator +{ +public: + static const std::size_t rank = IndexType::rank; + using iterator_category = std::random_access_iterator_tag; + using value_type = IndexType; + using difference_type = std::ptrdiff_t; + using pointer = value_type*; + using reference = value_type&; + using index_type = value_type; + using index_size_type = typename IndexType::value_type; + template + explicit bounds_iterator(const Bounds& bnd, value_type curr) noexcept + : boundary_(bnd.index_bounds()), curr_(std::move(curr)) + { + static_assert(is_bounds::value, "Bounds type must be provided"); + } + + constexpr reference operator*() const noexcept { return curr_; } + + constexpr pointer operator->() const noexcept { return &curr_; } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute + constexpr bounds_iterator& operator++() noexcept + + { + for (std::size_t i = rank; i-- > 0;) + { + if (curr_[i] < boundary_[i] - 1) + { + curr_[i]++; + return *this; + } + curr_[i] = 0; + } + // If we're here we've wrapped over - set to past-the-end. + curr_ = boundary_; + return *this; + } + + constexpr bounds_iterator operator++(int) noexcept + { + auto ret = *this; + ++(*this); + return ret; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr bounds_iterator& operator--() + { + if (!less(curr_, boundary_)) + { + // if at the past-the-end, set to last element + for (std::size_t i = 0; i < rank; ++i) { curr_[i] = boundary_[i] - 1; } + return *this; + } + for (std::size_t i = rank; i-- > 0;) + { + if (curr_[i] >= 1) + { + curr_[i]--; + return *this; + } + curr_[i] = boundary_[i] - 1; + } + // If we're here the preconditions were violated + // "pre: there exists s such that r == ++s" + Expects(false); + return *this; + } + + constexpr bounds_iterator operator--(int) noexcept + { + auto ret = *this; + --(*this); + return ret; + } + + constexpr bounds_iterator operator+(difference_type n) const noexcept + { + bounds_iterator ret{*this}; + return ret += n; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr bounds_iterator& operator+=(difference_type n) + { + auto linear_idx = linearize(curr_) + n; + std::remove_const_t stride = 0; + stride[rank - 1] = 1; + for (std::size_t i = rank - 1; i-- > 0;) { stride[i] = stride[i + 1] * boundary_[i + 1]; } + for (std::size_t i = 0; i < rank; ++i) + { + curr_[i] = linear_idx / stride[i]; + linear_idx = linear_idx % stride[i]; + } + // index is out of bounds of the array + Expects(!less(curr_, index_type{}) && !less(boundary_, curr_)); + return *this; + } + + constexpr bounds_iterator operator-(difference_type n) const noexcept + { + bounds_iterator ret{*this}; + return ret -= n; + } + + constexpr bounds_iterator& operator-=(difference_type n) noexcept { return *this += -n; } + + constexpr difference_type operator-(const bounds_iterator& rhs) const noexcept + { + return linearize(curr_) - linearize(rhs.curr_); + } + + constexpr value_type operator[](difference_type n) const noexcept { return *(*this + n); } + + constexpr bool operator==(const bounds_iterator& rhs) const noexcept + { + return curr_ == rhs.curr_; + } + + + constexpr bool operator!=(const bounds_iterator& rhs) const noexcept { return !(*this == rhs); } + + constexpr bool operator<(const bounds_iterator& rhs) const noexcept + { + return less(curr_, rhs.curr_); + } + + constexpr bool operator<=(const bounds_iterator& rhs) const noexcept { return !(rhs < *this); } + + constexpr bool operator>(const bounds_iterator& rhs) const noexcept { return rhs < *this; } + + constexpr bool operator>=(const bounds_iterator& rhs) const noexcept { return !(rhs > *this); } + + void swap(bounds_iterator& rhs) noexcept + { + std::swap(boundary_, rhs.boundary_); + std::swap(curr_, rhs.curr_); + } + +private: + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr bool less(index_type& one, index_type& other) const noexcept + { + for (std::size_t i = 0; i < rank; ++i) + { + if (one[i] < other[i]) return true; + } + return false; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + constexpr index_size_type linearize(const value_type& idx) const noexcept + { + // TODO: Smarter impl. + // Check if past-the-end + index_size_type multiplier = 1; + index_size_type res = 0; + if (!less(idx, boundary_)) + { + res = 1; + for (std::size_t i = rank; i-- > 0;) + { + res += (idx[i] - 1) * multiplier; + multiplier *= boundary_[i]; + } + } + else + { + for (std::size_t i = rank; i-- > 0;) + { + res += idx[i] * multiplier; + multiplier *= boundary_[i]; + } + } + return res; + } + + value_type boundary_; + std::remove_const_t curr_; +}; + +template +bounds_iterator operator+(typename bounds_iterator::difference_type n, + const bounds_iterator& rhs) noexcept +{ + return rhs + n; +} + +namespace details +{ + template + constexpr std::enable_if_t< + std::is_same::value, + typename Bounds::index_type> + make_stride(const Bounds& bnd) noexcept + { + return bnd.strides(); + } + + // Make a stride vector from bounds, assuming contiguous memory. + template + constexpr std::enable_if_t< + std::is_same::value, + typename Bounds::index_type> + make_stride(const Bounds& bnd) noexcept + { + auto extents = bnd.index_bounds(); + typename Bounds::size_type stride[Bounds::rank] = {}; + + stride[Bounds::rank - 1] = 1; + for (std::size_t i = 1; i < Bounds::rank; ++i) + { + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute + stride[Bounds::rank - i - 1] = stride[Bounds::rank - i] * extents[Bounds::rank - i]; + } + return {stride}; + } + + template + void verifyBoundsReshape(const BoundsSrc& src, const BoundsDest& dest) + { + static_assert(is_bounds::value && is_bounds::value, + "The src type and dest type must be bounds"); + static_assert(std::is_same::value, + "The source type must be a contiguous bounds"); + static_assert(BoundsDest::static_size == dynamic_range || + BoundsSrc::static_size == dynamic_range || + BoundsDest::static_size == BoundsSrc::static_size, + "The source bounds must have same size as dest bounds"); + Expects(src.size() == dest.size()); + } + +} // namespace details + +template +class contiguous_span_iterator; +template +class general_span_iterator; + +template +struct dim_t +{ + static const std::ptrdiff_t value = DimSize; +}; +template <> +struct dim_t +{ + static const std::ptrdiff_t value = dynamic_range; + const std::ptrdiff_t dvalue; + constexpr dim_t(std::ptrdiff_t size) noexcept : dvalue(size) {} +}; + +template = 0)>> +constexpr dim_t dim() noexcept +{ + return dim_t(); +} + +template > +constexpr dim_t dim(std::ptrdiff_t n) noexcept +{ + return dim_t<>(n); +} + +template +class multi_span; +template +class strided_span; + +namespace details +{ + template + struct SpanTypeTraits + { + using value_type = T; + using size_type = std::size_t; + }; + + template + struct SpanTypeTraits::type> + { + using value_type = typename Traits::span_traits::value_type; + using size_type = typename Traits::span_traits::size_type; + }; + + template + struct SpanArrayTraits + { + using type = multi_span; + using value_type = T; + using bounds_type = static_bounds; + using pointer = T*; + using reference = T&; + }; + template + struct SpanArrayTraits : SpanArrayTraits + { + }; + + template + BoundsType newBoundsHelperImpl(std::ptrdiff_t totalSize, std::true_type) // dynamic size + { + Expects(totalSize >= 0 && totalSize <= PTRDIFF_MAX); + return BoundsType{totalSize}; + } + template + BoundsType newBoundsHelperImpl(std::ptrdiff_t totalSize, std::false_type) // static size + { + Expects(BoundsType::static_size <= totalSize); + return {}; + } + template + BoundsType newBoundsHelper(std::ptrdiff_t totalSize) + { + static_assert(BoundsType::dynamic_rank <= 1, "dynamic rank must less or equal to 1"); + return newBoundsHelperImpl( + totalSize, std::integral_constant()); + } + + struct Sep + { + }; + + template + T static_as_multi_span_helper(Sep, Args... args) + { + return T{narrow_cast(args)...}; + } + template + std::enable_if_t< + !std::is_same>::value && !std::is_same::value, T> + static_as_multi_span_helper(Arg, Args... args) + { + return static_as_multi_span_helper(args...); + } + template + T static_as_multi_span_helper(dim_t val, Args... args) + { + return static_as_multi_span_helper(args..., val.dvalue); + } + + template + struct static_as_multi_span_static_bounds_helper + { + using type = static_bounds<(Dimensions::value)...>; + }; + + template + struct is_multi_span_oracle : std::false_type + { + }; + + template + struct is_multi_span_oracle> + : std::true_type + { + }; + + template + struct is_multi_span_oracle> : std::true_type + { + }; + + template + struct is_multi_span : is_multi_span_oracle> + { + }; +} // namespace details + +template +class multi_span +{ + // TODO do we still need this? + template + friend class multi_span; + +public: + using bounds_type = static_bounds; + static const std::size_t Rank = bounds_type::rank; + using size_type = typename bounds_type::size_type; + using index_type = typename bounds_type::index_type; + using value_type = ValueType; + using const_value_type = std::add_const_t; + using pointer = std::add_pointer_t; + using reference = std::add_lvalue_reference_t; + using iterator = contiguous_span_iterator; + using const_span = multi_span; + using const_iterator = contiguous_span_iterator; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + using sliced_type = + std::conditional_t>; + +private: + pointer data_; + bounds_type bounds_; + + friend iterator; + friend const_iterator; + +public: + // default constructor - same as constructing from nullptr_t + GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive + constexpr multi_span() noexcept + : multi_span(nullptr, bounds_type{}) + { + static_assert(bounds_type::dynamic_rank != 0 || + (bounds_type::dynamic_rank == 0 && bounds_type::static_size == 0), + "Default construction of multi_span only possible " + "for dynamic or fixed, zero-length spans."); + } + + // construct from nullptr - get an empty multi_span + GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive + constexpr multi_span(std::nullptr_t) noexcept + : multi_span(nullptr, bounds_type{}) + { + static_assert(bounds_type::dynamic_rank != 0 || + (bounds_type::dynamic_rank == 0 && bounds_type::static_size == 0), + "nullptr_t construction of multi_span only possible " + "for dynamic or fixed, zero-length spans."); + } + + // construct from nullptr with size of 0 (helps with template function calls) + template ::value>> + + // GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive // TODO: parser bug + constexpr multi_span(std::nullptr_t, IntType size) : multi_span(nullptr, bounds_type{}) + { + static_assert(bounds_type::dynamic_rank != 0 || + (bounds_type::dynamic_rank == 0 && bounds_type::static_size == 0), + "nullptr_t construction of multi_span only possible " + "for dynamic or fixed, zero-length spans."); + Expects(size == 0); + } + + // construct from a single element + + GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive + constexpr multi_span(reference data) noexcept + : multi_span(&data, bounds_type{1}) + { + static_assert(bounds_type::dynamic_rank > 0 || bounds_type::static_size == 0 || + bounds_type::static_size == 1, + "Construction from a single element only possible " + "for dynamic or fixed spans of length 0 or 1."); + } + + // prevent constructing from temporaries for single-elements + constexpr multi_span(value_type&&) = delete; + + // construct from pointer + length + GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive + constexpr multi_span(pointer ptr, size_type size) + : multi_span(ptr, bounds_type{size}) + {} + + // construct from pointer + length - multidimensional + constexpr multi_span(pointer data, bounds_type bounds) + : data_(data), bounds_(std::move(bounds)) + { + Expects((bounds_.size() > 0 && data != nullptr) || bounds_.size() == 0); + } + + // construct from begin,end pointer pair + template ::value && + details::LessThan::value>> + constexpr multi_span(pointer begin, Ptr end) + : multi_span(begin, + details::newBoundsHelper(static_cast(end) - begin)) + { + Expects(begin != nullptr && end != nullptr && begin <= static_cast(end)); + } + + // construct from n-dimensions static array + template > + constexpr multi_span(T (&arr)[N]) + : multi_span(reinterpret_cast(arr), bounds_type{typename Helper::bounds_type{}}) + { + static_assert(std::is_convertible::value, + "Cannot convert from source type to target multi_span type."); + static_assert(std::is_convertible::value, + "Cannot construct a multi_span from an array with fewer elements."); + } + + // construct from n-dimensions dynamic array (e.g. new int[m][4]) + // (precedence will be lower than the 1-dimension pointer) + template > + constexpr multi_span(T* const& data, size_type size) + : multi_span(reinterpret_cast(data), typename Helper::bounds_type{size}) + { + static_assert(std::is_convertible::value, + "Cannot convert from source type to target multi_span type."); + } + + // construct from std::array + template + constexpr multi_span(std::array& arr) + : multi_span(arr.data(), bounds_type{static_bounds{}}) + { + static_assert( + std::is_convertible(*)[]>::value, + "Cannot convert from source type to target multi_span type."); + static_assert(std::is_convertible, bounds_type>::value, + "You cannot construct a multi_span from a std::array of smaller size."); + } + + // construct from const std::array + template + constexpr multi_span(const std::array& arr) + : multi_span(arr.data(), bounds_type{static_bounds{}}) + { + static_assert( + std::is_convertible(*)[]>::value, + "Cannot convert from source type to target multi_span type."); + static_assert(std::is_convertible, bounds_type>::value, + "You cannot construct a multi_span from a std::array of smaller size."); + } + + // prevent constructing from temporary std::array + template + constexpr multi_span(std::array&& arr) = delete; + + // construct from containers + // future: could use contiguous_iterator_traits to identify only contiguous containers + // type-requirements: container must have .size(), operator[] which are value_type compatible + template ::value && + std::is_convertible::value && + std::is_same().size(), + *std::declval().data())>, + DataType>::value>> + constexpr multi_span(Cont& cont) + : multi_span(static_cast(cont.data()), + details::newBoundsHelper(narrow_cast(cont.size()))) + {} + + // prevent constructing from temporary containers + template ::value && + std::is_convertible::value && + std::is_same().size(), + *std::declval().data())>, + DataType>::value>> + explicit constexpr multi_span(Cont&& cont) = delete; + + // construct from a convertible multi_span + template , + typename = std::enable_if_t::value && + std::is_convertible::value>> + constexpr multi_span(multi_span other) + : data_(other.data_), bounds_(other.bounds_) + {} + + // trivial copy and move + constexpr multi_span(const multi_span&) = default; + constexpr multi_span(multi_span&&) = default; + + // trivial assignment + constexpr multi_span& operator=(const multi_span&) = default; + constexpr multi_span& operator=(multi_span&&) = default; + + // first() - extract the first Count elements into a new multi_span + template + + constexpr multi_span first() const + { + static_assert(Count >= 0, "Count must be >= 0."); + static_assert(bounds_type::static_size == dynamic_range || + Count <= bounds_type::static_size, + "Count is out of bounds."); + + Expects(bounds_type::static_size != dynamic_range || Count <= this->size()); + return {this->data(), Count}; + } + + // first() - extract the first count elements into a new multi_span + constexpr multi_span first(size_type count) const + { + Expects(count >= 0 && count <= this->size()); + return {this->data(), count}; + } + + // last() - extract the last Count elements into a new multi_span + template + constexpr multi_span last() const + { + static_assert(Count >= 0, "Count must be >= 0."); + static_assert(bounds_type::static_size == dynamic_range || + Count <= bounds_type::static_size, + "Count is out of bounds."); + + Expects(bounds_type::static_size != dynamic_range || Count <= this->size()); + return {this->data() + this->size() - Count, Count}; + } + + // last() - extract the last count elements into a new multi_span + constexpr multi_span last(size_type count) const + { + Expects(count >= 0 && count <= this->size()); + return {this->data() + this->size() - count, count}; + } + + // subspan() - create a subview of Count elements starting at Offset + template + constexpr multi_span subspan() const + { + static_assert(Count >= 0, "Count must be >= 0."); + static_assert(Offset >= 0, "Offset must be >= 0."); + static_assert(bounds_type::static_size == dynamic_range || + ((Offset <= bounds_type::static_size) && + Count <= bounds_type::static_size - Offset), + "You must describe a sub-range within bounds of the multi_span."); + + Expects(bounds_type::static_size != dynamic_range || + (Offset <= this->size() && Count <= this->size() - Offset)); + return {this->data() + Offset, Count}; + } + + // subspan() - create a subview of count elements starting at offset + // supplying dynamic_range for count will consume all available elements from offset + constexpr multi_span subspan(size_type offset, + size_type count = dynamic_range) const + { + Expects((offset >= 0 && offset <= this->size()) && + (count == dynamic_range || (count <= this->size() - offset))); + return {this->data() + offset, count == dynamic_range ? this->length() - offset : count}; + } + + // section - creates a non-contiguous, strided multi_span from a contiguous one + constexpr strided_span section(index_type origin, + index_type extents) const + { + const size_type size = this->bounds().total_size() - this->bounds().linearize(origin); + return {&this->operator[](origin), size, + strided_bounds{extents, details::make_stride(bounds())}}; + } + + // length of the multi_span in elements + constexpr size_type size() const noexcept { return bounds_.size(); } + + // length of the multi_span in elements + constexpr size_type length() const noexcept { return this->size(); } + + // length of the multi_span in bytes + constexpr size_type size_bytes() const noexcept + { + return narrow_cast(sizeof(value_type)) * this->size(); + } + + // length of the multi_span in bytes + constexpr size_type length_bytes() const noexcept { return this->size_bytes(); } + + constexpr bool empty() const noexcept { return this->size() == 0; } + + static constexpr std::size_t rank() { return Rank; } + + template + constexpr size_type extent() const noexcept + { + static_assert(Dim < Rank, + "Dimension should be less than rank (dimension count starts from 0)."); + return bounds_.template extent(); + } + + template + constexpr size_type extent(IntType dim) const + { + return bounds_.extent(dim); + } + + constexpr bounds_type bounds() const noexcept { return bounds_; } + + constexpr pointer data() const noexcept { return data_; } + + template + constexpr reference operator()(FirstIndex idx) + { + return this->operator[](narrow_cast(idx)); + } + + template + constexpr reference operator()(FirstIndex firstIndex, OtherIndices... indices) + { + const index_type idx = {narrow_cast(firstIndex), + narrow_cast(indices)...}; + return this->operator[](idx); + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr reference operator[](const index_type& idx) const + { + return data_[bounds_.linearize(idx)]; + } + + template 1), typename Ret = std::enable_if_t> + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr Ret operator[](size_type idx) const + { + Expects(idx >= 0 && idx < bounds_.size()); // index is out of bounds of the array + const size_type ridx = idx * bounds_.stride(); + + // index is out of bounds of the underlying data + Expects(ridx < bounds_.total_size()); + return Ret{data_ + ridx, bounds_.slice()}; + } + + constexpr iterator begin() const noexcept { return iterator{this, true}; } + + constexpr iterator end() const noexcept { return iterator{this, false}; } + + GSL_SUPPRESS(type.1) // NO-FORMAT: attribute + constexpr const_iterator cbegin() const noexcept + { + return const_iterator{reinterpret_cast(this), true}; + } + + constexpr const_iterator cend() const noexcept + { + return const_iterator{reinterpret_cast(this), false}; + } + + constexpr reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; } + + constexpr reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; } + + constexpr const_reverse_iterator crbegin() const noexcept + { + return const_reverse_iterator{cend()}; + } + + constexpr const_reverse_iterator crend() const noexcept + { + return const_reverse_iterator{cbegin()}; + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator==(const multi_span& other) const + { + return bounds_.size() == other.bounds_.size() && + (data_ == other.data_ || std::equal(this->begin(), this->end(), other.begin())); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator!=(const multi_span& other) const + { + return !(*this == other); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator<(const multi_span& other) const + { + return std::lexicographical_compare(this->begin(), this->end(), other.begin(), other.end()); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator<=(const multi_span& other) const + { + return !(other < *this); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator>(const multi_span& other) const + noexcept + { + return (other < *this); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator>=(const multi_span& other) const + { + return !(*this < other); + } +}; + +// +// Free functions for manipulating spans +// + +// reshape a multi_span into a different dimensionality +// DimCount and Enabled here are workarounds for a bug in MSVC 2015 +template 0), typename = std::enable_if_t> +constexpr auto as_multi_span(SpanType s, Dimensions2... dims) + -> multi_span +{ + static_assert(details::is_multi_span::value, + "Variadic as_multi_span() is for reshaping existing spans."); + using BoundsType = + typename multi_span::bounds_type; + const auto tobounds = details::static_as_multi_span_helper(dims..., details::Sep{}); + details::verifyBoundsReshape(s.bounds(), tobounds); + return {s.data(), tobounds}; +} + +// convert a multi_span to a multi_span +template +multi_span as_bytes(multi_span s) noexcept +{ + static_assert(std::is_trivial>::value, + "The value_type of multi_span must be a trivial type."); + return {reinterpret_cast(s.data()), s.size_bytes()}; +} + +// convert a multi_span to a multi_span (a writeable byte multi_span) +// this is not currently a portable function that can be relied upon to work +// on all implementations. It should be considered an experimental extension +// to the standard GSL interface. +template +multi_span as_writeable_bytes(multi_span s) noexcept +{ + static_assert(std::is_trivial>::value, + "The value_type of multi_span must be a trivial type."); + return {reinterpret_cast(s.data()), s.size_bytes()}; +} + +// convert a multi_span to a multi_span +// this is not currently a portable function that can be relied upon to work +// on all implementations. It should be considered an experimental extension +// to the standard GSL interface. +template +constexpr auto as_multi_span(multi_span s) -> multi_span< + const U, static_cast( + multi_span::bounds_type::static_size != dynamic_range + ? (static_cast( + multi_span::bounds_type::static_size) / + sizeof(U)) + : dynamic_range)> +{ + using ConstByteSpan = multi_span; + static_assert( + std::is_trivial>::value && + (ConstByteSpan::bounds_type::static_size == dynamic_range || + ConstByteSpan::bounds_type::static_size % narrow_cast(sizeof(U)) == 0), + "Target type must be a trivial type and its size must match the byte array size"); + + Expects((s.size_bytes() % narrow_cast(sizeof(U))) == 0 && + (s.size_bytes() / narrow_cast(sizeof(U))) < PTRDIFF_MAX); + return {reinterpret_cast(s.data()), + s.size_bytes() / narrow_cast(sizeof(U))}; +} + +// convert a multi_span to a multi_span +// this is not currently a portable function that can be relied upon to work +// on all implementations. It should be considered an experimental extension +// to the standard GSL interface. +template +constexpr auto as_multi_span(multi_span s) + -> multi_span( + multi_span::bounds_type::static_size != dynamic_range + ? static_cast( + multi_span::bounds_type::static_size) / + sizeof(U) + : dynamic_range)> +{ + using ByteSpan = multi_span; + static_assert(std::is_trivial>::value && + (ByteSpan::bounds_type::static_size == dynamic_range || + ByteSpan::bounds_type::static_size % sizeof(U) == 0), + "Target type must be a trivial type and its size must match the byte array size"); + + Expects((s.size_bytes() % sizeof(U)) == 0); + return {reinterpret_cast(s.data()), + s.size_bytes() / narrow_cast(sizeof(U))}; +} + +template +constexpr auto as_multi_span(T* const& ptr, dim_t... args) + -> multi_span, Dimensions...> +{ + return {reinterpret_cast*>(ptr), + details::static_as_multi_span_helper>(args..., + details::Sep{})}; +} + +template +constexpr auto as_multi_span(T* arr, std::ptrdiff_t len) -> + typename details::SpanArrayTraits::type +{ + return {reinterpret_cast*>(arr), len}; +} + +template +constexpr auto as_multi_span(T (&arr)[N]) -> typename details::SpanArrayTraits::type +{ + return {arr}; +} + +template +constexpr multi_span as_multi_span(const std::array& arr) +{ + return {arr}; +} + +template +constexpr multi_span as_multi_span(const std::array&&) = delete; + +template +constexpr multi_span as_multi_span(std::array& arr) +{ + return {arr}; +} + +template +constexpr multi_span as_multi_span(T* begin, T* end) +{ + return {begin, end}; +} + +template +constexpr auto as_multi_span(Cont& arr) -> std::enable_if_t< + !details::is_multi_span>::value, + multi_span, dynamic_range>> +{ + Expects(arr.size() < PTRDIFF_MAX); + return {arr.data(), narrow_cast(arr.size())}; +} + +template +constexpr auto as_multi_span(Cont&& arr) -> std::enable_if_t< + !details::is_multi_span>::value, + multi_span, dynamic_range>> = delete; + +// from basic_string which doesn't have nonconst .data() member like other contiguous containers +template +GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute +constexpr auto as_multi_span(std::basic_string& str) + -> multi_span +{ + Expects(str.size() < PTRDIFF_MAX); + return {&str[0], narrow_cast(str.size())}; +} + +// strided_span is an extension that is not strictly part of the GSL at this time. +// It is kept here while the multidimensional interface is still being defined. +template +class strided_span +{ +public: + using bounds_type = strided_bounds; + using size_type = typename bounds_type::size_type; + using index_type = typename bounds_type::index_type; + using value_type = ValueType; + using const_value_type = std::add_const_t; + using pointer = std::add_pointer_t; + using reference = std::add_lvalue_reference_t; + using iterator = general_span_iterator; + using const_strided_span = strided_span; + using const_iterator = general_span_iterator; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + using sliced_type = + std::conditional_t>; + +private: + pointer data_; + bounds_type bounds_; + + friend iterator; + friend const_iterator; + template + friend class strided_span; + +public: + // from raw data + constexpr strided_span(pointer ptr, size_type size, bounds_type bounds) + : data_(ptr), bounds_(std::move(bounds)) + { + Expects((bounds_.size() > 0 && ptr != nullptr) || bounds_.size() == 0); + // Bounds cross data boundaries + Expects(this->bounds().total_size() <= size); + GSL_SUPPRESS(type.4) // NO-FORMAT: attribute // TODO: false positive + (void) size; + } + + // from static array of size N + template + constexpr strided_span(value_type (&values)[N], bounds_type bounds) + : strided_span(values, N, std::move(bounds)) + {} + + // from array view + template ::value, + typename = std::enable_if_t> + constexpr strided_span(multi_span av, bounds_type bounds) + : strided_span(av.data(), av.bounds().total_size(), std::move(bounds)) + {} + + // convertible + template ::value>> + constexpr strided_span(const strided_span& other) + : data_(other.data_), bounds_(other.bounds_) + {} + + // convert from bytes + template + constexpr strided_span< + typename std::enable_if::value, OtherValueType>::type, + Rank> + as_strided_span() const + { + static_assert((sizeof(OtherValueType) >= sizeof(value_type)) && + (sizeof(OtherValueType) % sizeof(value_type) == 0), + "OtherValueType should have a size to contain a multiple of ValueTypes"); + auto d = narrow_cast(sizeof(OtherValueType) / sizeof(value_type)); + + const size_type size = this->bounds().total_size() / d; + + GSL_SUPPRESS(type.3) // NO-FORMAT: attribute + return {const_cast(reinterpret_cast(this->data())), + size, + bounds_type{resize_extent(this->bounds().index_bounds(), d), + resize_stride(this->bounds().strides(), d)}}; + } + + constexpr strided_span section(index_type origin, index_type extents) const + { + const size_type size = this->bounds().total_size() - this->bounds().linearize(origin); + return {&this->operator[](origin), size, + bounds_type{extents, details::make_stride(bounds())}}; + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr reference operator[](const index_type& idx) const + { + return data_[bounds_.linearize(idx)]; + } + + template 1), typename Ret = std::enable_if_t> + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr Ret operator[](size_type idx) const + { + Expects(idx < bounds_.size()); // index is out of bounds of the array + const size_type ridx = idx * bounds_.stride(); + + // index is out of bounds of the underlying data + Expects(ridx < bounds_.total_size()); + return {data_ + ridx, bounds_.slice().total_size(), bounds_.slice()}; + } + + constexpr bounds_type bounds() const noexcept { return bounds_; } + + template + constexpr size_type extent() const noexcept + { + static_assert(Dim < Rank, + "dimension should be less than Rank (dimension count starts from 0)"); + return bounds_.template extent(); + } + + constexpr size_type size() const noexcept { return bounds_.size(); } + + constexpr pointer data() const noexcept { return data_; } + + constexpr bool empty() const noexcept { return this->size() == 0; } + + constexpr explicit operator bool() const noexcept { return data_ != nullptr; } + + constexpr iterator begin() const { return iterator{this, true}; } + + constexpr iterator end() const { return iterator{this, false}; } + + constexpr const_iterator cbegin() const + { + return const_iterator{reinterpret_cast(this), true}; + } + + constexpr const_iterator cend() const + { + return const_iterator{reinterpret_cast(this), false}; + } + + constexpr reverse_iterator rbegin() const { return reverse_iterator{end()}; } + + constexpr reverse_iterator rend() const { return reverse_iterator{begin()}; } + + constexpr const_reverse_iterator crbegin() const { return const_reverse_iterator{cend()}; } + + constexpr const_reverse_iterator crend() const { return const_reverse_iterator{cbegin()}; } + + template , + std::remove_cv_t>::value>> + constexpr bool operator==(const strided_span& other) const + { + return bounds_.size() == other.bounds_.size() && + (data_ == other.data_ || std::equal(this->begin(), this->end(), other.begin())); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator!=(const strided_span& other) const + { + return !(*this == other); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator<(const strided_span& other) const + { + return std::lexicographical_compare(this->begin(), this->end(), other.begin(), other.end()); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator<=(const strided_span& other) const + { + return !(other < *this); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator>(const strided_span& other) const + { + return (other < *this); + } + + template , + std::remove_cv_t>::value>> + constexpr bool operator>=(const strided_span& other) const + { + return !(*this < other); + } + +private: + static index_type resize_extent(const index_type& extent, std::ptrdiff_t d) + { + // The last dimension of the array needs to contain a multiple of new type elements + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + Expects(extent[Rank - 1] >= d && (extent[Rank - 1] % d == 0)); + + index_type ret = extent; + ret[Rank - 1] /= d; + + return ret; + } + + template > + static index_type resize_stride(const index_type& strides, std::ptrdiff_t, void* = nullptr) + { + // Only strided arrays with regular strides can be resized + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + Expects(strides[Rank - 1] == 1); + + return strides; + } + + template 1), typename = std::enable_if_t> + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + static index_type resize_stride(const index_type& strides, std::ptrdiff_t d) + { + // Only strided arrays with regular strides can be resized + Expects(strides[Rank - 1] == 1); + // The strides must have contiguous chunks of + // memory that can contain a multiple of new type elements + Expects(strides[Rank - 2] >= d && (strides[Rank - 2] % d == 0)); + + for (std::size_t i = Rank - 1; i > 0; --i) + { + // Only strided arrays with regular strides can be resized + Expects((strides[i - 1] >= strides[i]) && (strides[i - 1] % strides[i] == 0)); + } + + index_type ret = strides / d; + ret[Rank - 1] = 1; + + return ret; + } +}; + +template +class contiguous_span_iterator +{ +public: + using iterator_category = std::random_access_iterator_tag; + using value_type = typename Span::value_type; + using difference_type = std::ptrdiff_t; + using pointer = value_type*; + using reference = value_type&; + +private: + template + friend class multi_span; + + pointer data_; + const Span* m_validator; + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + void validateThis() const { + // iterator is out of range of the array + Expects(data_ >= m_validator->data_ && data_ < m_validator->data_ + m_validator->size()); + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + contiguous_span_iterator(const Span* container, bool isbegin) + : data_(isbegin ? container->data_ : container->data_ + container->size()) + , m_validator(container) + {} + +public: + reference operator*() const + { + validateThis(); + return *data_; + } + pointer operator->() const + { + validateThis(); + return data_; + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + contiguous_span_iterator& operator++() noexcept + { + ++data_; + return *this; + } + contiguous_span_iterator operator++(int) noexcept + { + auto ret = *this; + ++(*this); + return ret; + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + contiguous_span_iterator& operator--() noexcept + { + --data_; + return *this; + } + contiguous_span_iterator operator--(int) noexcept + { + auto ret = *this; + --(*this); + return ret; + } + contiguous_span_iterator operator+(difference_type n) const noexcept + { + contiguous_span_iterator ret{*this}; + return ret += n; + } + contiguous_span_iterator& operator+=(difference_type n) noexcept + { + data_ += n; + return *this; + } + contiguous_span_iterator operator-(difference_type n) const noexcept + { + contiguous_span_iterator ret{*this}; + return ret -= n; + } + + contiguous_span_iterator& operator-=(difference_type n) { return *this += -n; } + difference_type operator-(const contiguous_span_iterator& rhs) const + { + Expects(m_validator == rhs.m_validator); + return data_ - rhs.data_; + } + reference operator[](difference_type n) const { return *(*this + n); } + bool operator==(const contiguous_span_iterator& rhs) const + { + Expects(m_validator == rhs.m_validator); + return data_ == rhs.data_; + } + + bool operator!=(const contiguous_span_iterator& rhs) const { return !(*this == rhs); } + + bool operator<(const contiguous_span_iterator& rhs) const + { + Expects(m_validator == rhs.m_validator); + return data_ < rhs.data_; + } + + bool operator<=(const contiguous_span_iterator& rhs) const { return !(rhs < *this); } + bool operator>(const contiguous_span_iterator& rhs) const { return rhs < *this; } + bool operator>=(const contiguous_span_iterator& rhs) const { return !(rhs > *this); } + + void swap(contiguous_span_iterator& rhs) noexcept + { + std::swap(data_, rhs.data_); + std::swap(m_validator, rhs.m_validator); + } +}; + +template +contiguous_span_iterator operator+(typename contiguous_span_iterator::difference_type n, + const contiguous_span_iterator& rhs) noexcept +{ + return rhs + n; +} + +template +class general_span_iterator { +public: + using iterator_category = std::random_access_iterator_tag; + using value_type = typename Span::value_type; + using difference_type = std::ptrdiff_t; + using pointer = value_type*; + using reference = value_type&; + +private: + template + friend class strided_span; + + const Span* m_container; + typename Span::bounds_type::iterator m_itr; + general_span_iterator(const Span* container, bool isbegin) + : m_container(container) + , m_itr(isbegin ? m_container->bounds().begin() : m_container->bounds().end()) + {} + +public: + reference operator*() noexcept { return (*m_container)[*m_itr]; } + pointer operator->() noexcept { return &(*m_container)[*m_itr]; } + general_span_iterator& operator++() noexcept + { + ++m_itr; + return *this; + } + general_span_iterator operator++(int) noexcept + { + auto ret = *this; + ++(*this); + return ret; + } + general_span_iterator& operator--() noexcept + { + --m_itr; + return *this; + } + general_span_iterator operator--(int) noexcept + { + auto ret = *this; + --(*this); + return ret; + } + general_span_iterator operator+(difference_type n) const noexcept + { + general_span_iterator ret{*this}; + return ret += n; + } + general_span_iterator& operator+=(difference_type n) noexcept + { + m_itr += n; + return *this; + } + general_span_iterator operator-(difference_type n) const noexcept + { + general_span_iterator ret{*this}; + return ret -= n; + } + general_span_iterator& operator-=(difference_type n) noexcept { return *this += -n; } + difference_type operator-(const general_span_iterator& rhs) const + { + Expects(m_container == rhs.m_container); + return m_itr - rhs.m_itr; + } + + GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute + value_type operator[](difference_type n) const { return (*m_container)[m_itr[n]]; } + + bool operator==(const general_span_iterator& rhs) const + { + Expects(m_container == rhs.m_container); + return m_itr == rhs.m_itr; + } + bool operator!=(const general_span_iterator& rhs) const { return !(*this == rhs); } + bool operator<(const general_span_iterator& rhs) const + { + Expects(m_container == rhs.m_container); + return m_itr < rhs.m_itr; + } + bool operator<=(const general_span_iterator& rhs) const { return !(rhs < *this); } + bool operator>(const general_span_iterator& rhs) const { return rhs < *this; } + bool operator>=(const general_span_iterator& rhs) const { return !(rhs > *this); } + void swap(general_span_iterator& rhs) noexcept + { + std::swap(m_itr, rhs.m_itr); + std::swap(m_container, rhs.m_container); + } +}; + +template +general_span_iterator operator+(typename general_span_iterator::difference_type n, + const general_span_iterator& rhs) noexcept +{ + return rhs + n; +} + +} // namespace gsl + +#if defined(_MSC_VER) && !defined(__clang__) +#if _MSC_VER < 1910 + +#undef constexpr +#pragma pop_macro("constexpr") +#endif // _MSC_VER < 1910 + +#pragma warning(pop) + +#endif // _MSC_VER + +#if defined(__GNUC__) && __GNUC__ > 6 +#pragma GCC diagnostic pop +#endif // __GNUC__ > 6 + +#endif // GSL_MULTI_SPAN_H diff --git a/src/algorithms/libs/gsl/include/gsl/pointers b/src/algorithms/libs/gsl/include/gsl/pointers new file mode 100644 index 000000000..0f2987a0a --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/pointers @@ -0,0 +1,294 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_POINTERS_H +#define GSL_POINTERS_H + +#include // for Ensures, Expects + +#include // for forward +#include // for ptrdiff_t, nullptr_t, ostream, size_t +#include // for shared_ptr, unique_ptr +#include // for hash +#include // for enable_if_t, is_convertible, is_assignable + +#if defined(_MSC_VER) && _MSC_VER < 1910 && !defined(__clang__) +#pragma push_macro("constexpr") +#define constexpr /*constexpr*/ + +#endif // defined(_MSC_VER) && _MSC_VER < 1910 + +namespace gsl +{ + +// +// GSL.owner: ownership pointers +// +using std::unique_ptr; +using std::shared_ptr; + +// +// owner +// +// owner is designed as a bridge for code that must deal directly with owning pointers for some reason +// +// T must be a pointer type +// - disallow construction from any type other than pointer type +// +template ::value>> +using owner = T; + +// +// not_null +// +// Restricts a pointer or smart pointer to only hold non-null values. +// +// Has zero size overhead over T. +// +// If T is a pointer (i.e. T == U*) then +// - allow construction from U* +// - disallow construction from nullptr_t +// - disallow default construction +// - ensure construction from null U* fails +// - allow implicit conversion to U* +// +template +class not_null +{ +public: + static_assert(std::is_assignable::value, "T cannot be assigned nullptr."); + + template ::value>> + constexpr not_null(U&& u) : ptr_(std::forward(u)) + { + Expects(ptr_ != nullptr); + } + + template ::value>> + constexpr not_null(T u) : ptr_(u) + { + Expects(ptr_ != nullptr); + } + + template ::value>> + constexpr not_null(const not_null& other) : not_null(other.get()) + { + } + + not_null(not_null&& other) = default; + not_null(const not_null& other) = default; + not_null& operator=(const not_null& other) = default; + + constexpr T get() const + { + Ensures(ptr_ != nullptr); + return ptr_; + } + + constexpr operator T() const { return get(); } + constexpr T operator->() const { return get(); } + constexpr decltype(auto) operator*() const { return *get(); } + + // prevents compilation when someone attempts to assign a null pointer constant + not_null(std::nullptr_t) = delete; + not_null& operator=(std::nullptr_t) = delete; + + // unwanted operators...pointers only point to single objects! + not_null& operator++() = delete; + not_null& operator--() = delete; + not_null operator++(int) = delete; + not_null operator--(int) = delete; + not_null& operator+=(std::ptrdiff_t) = delete; + not_null& operator-=(std::ptrdiff_t) = delete; + void operator[](std::ptrdiff_t) const = delete; + +private: + T ptr_; +}; + +template +auto make_not_null(T&& t) { + return not_null>>{std::forward(t)}; +} + +template +std::ostream& operator<<(std::ostream& os, const not_null& val) +{ + os << val.get(); + return os; +} + +template +auto operator==(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() == rhs.get()) +{ + return lhs.get() == rhs.get(); +} + +template +auto operator!=(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() != rhs.get()) +{ + return lhs.get() != rhs.get(); +} + +template +auto operator<(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() < rhs.get()) +{ + return lhs.get() < rhs.get(); +} + +template +auto operator<=(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() <= rhs.get()) +{ + return lhs.get() <= rhs.get(); +} + +template +auto operator>(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() > rhs.get()) +{ + return lhs.get() > rhs.get(); +} + +template +auto operator>=(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() >= rhs.get()) +{ + return lhs.get() >= rhs.get(); +} + +// more unwanted operators +template +std::ptrdiff_t operator-(const not_null&, const not_null&) = delete; +template +not_null operator-(const not_null&, std::ptrdiff_t) = delete; +template +not_null operator+(const not_null&, std::ptrdiff_t) = delete; +template +not_null operator+(std::ptrdiff_t, const not_null&) = delete; + +} // namespace gsl + +namespace std +{ +template +struct hash> +{ + std::size_t operator()(const gsl::not_null& value) const { return hash{}(value); } +}; + +} // namespace std + +namespace gsl +{ + +// +// strict_not_null +// +// Restricts a pointer or smart pointer to only hold non-null values, +// +// - provides a strict (i.e. explicit contructor from T) wrapper of not_null +// - to be used for new code that wishes the design to be cleaner and make not_null +// checks intentional, or in old code that would like to make the transition. +// +// To make the transition from not_null, incrementally replace not_null +// by strict_not_null and fix compilation errors +// +// Expect to +// - remove all unneded conversions from raw pointer to not_null and back +// - make API clear by specifyning not_null in parameters where needed +// - remove unnesessary asserts +// +template +class strict_not_null: public not_null +{ +public: + + template ::value>> + constexpr explicit strict_not_null(U&& u) : + not_null(std::forward(u)) + {} + + template ::value>> + constexpr explicit strict_not_null(T u) : + not_null(u) + {} + + template ::value>> + constexpr strict_not_null(const not_null& other) : + not_null(other) + {} + + template ::value>> + constexpr strict_not_null(const strict_not_null& other) : + not_null(other) + {} + + strict_not_null(strict_not_null&& other) = default; + strict_not_null(const strict_not_null& other) = default; + strict_not_null& operator=(const strict_not_null& other) = default; + strict_not_null& operator=(const not_null& other) + { + not_null::operator=(other); + return *this; + } + + // prevents compilation when someone attempts to assign a null pointer constant + strict_not_null(std::nullptr_t) = delete; + strict_not_null& operator=(std::nullptr_t) = delete; + + // unwanted operators...pointers only point to single objects! + strict_not_null& operator++() = delete; + strict_not_null& operator--() = delete; + strict_not_null operator++(int) = delete; + strict_not_null operator--(int) = delete; + strict_not_null& operator+=(std::ptrdiff_t) = delete; + strict_not_null& operator-=(std::ptrdiff_t) = delete; + void operator[](std::ptrdiff_t) const = delete; +}; + +// more unwanted operators +template +std::ptrdiff_t operator-(const strict_not_null&, const strict_not_null&) = delete; +template +strict_not_null operator-(const strict_not_null&, std::ptrdiff_t) = delete; +template +strict_not_null operator+(const strict_not_null&, std::ptrdiff_t) = delete; +template +strict_not_null operator+(std::ptrdiff_t, const strict_not_null&) = delete; + +template +auto make_strict_not_null(T&& t) { + return strict_not_null>>{std::forward(t)}; +} + +} // namespace gsl + +namespace std +{ +template +struct hash> +{ + std::size_t operator()(const gsl::strict_not_null& value) const { return hash{}(value); } +}; + +} // namespace std + +#if defined(_MSC_VER) && _MSC_VER < 1910 && !defined(__clang__) + +#undef constexpr +#pragma pop_macro("constexpr") + +#endif // defined(_MSC_VER) && _MSC_VER < 1910 && !defined(__clang__) + +#endif // GSL_POINTERS_H diff --git a/src/algorithms/libs/gsl/include/gsl/span b/src/algorithms/libs/gsl/include/gsl/span new file mode 100644 index 000000000..b4da53216 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/span @@ -0,0 +1,793 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_SPAN_H +#define GSL_SPAN_H + +#include // for Expects +#include // for byte +#include // for narrow_cast, narrow + +#include // for lexicographical_compare +#include // for array +#include // for ptrdiff_t, size_t, nullptr_t +#include // for reverse_iterator, distance, random_access_... +#include +#include +#include // for enable_if_t, declval, is_convertible, inte... +#include +#include // for std::addressof + +#if defined(_MSC_VER) && !defined(__clang__) +#pragma warning(push) + +// turn off some warnings that are noisy about our Expects statements +#pragma warning(disable : 4127) // conditional expression is constant +#pragma warning(disable : 4702) // unreachable code + +// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. +#pragma warning(disable : 26495) // uninitalized member when constructor calls constructor +#pragma warning(disable : 26446) // parser bug does not allow attributes on some templates + +#if _MSC_VER < 1910 +#pragma push_macro("constexpr") +#define constexpr /*constexpr*/ +#define GSL_USE_STATIC_CONSTEXPR_WORKAROUND + +#endif // _MSC_VER < 1910 +#endif // _MSC_VER + +// See if we have enough C++17 power to use a static constexpr data member +// without needing an out-of-line definition +#if !(defined(__cplusplus) && (__cplusplus >= 201703L)) +#define GSL_USE_STATIC_CONSTEXPR_WORKAROUND +#endif // !(defined(__cplusplus) && (__cplusplus >= 201703L)) + +// GCC 7 does not like the signed unsigned missmatch (size_t ptrdiff_t) +// While there is a conversion from signed to unsigned, it happens at +// compiletime, so the compiler wouldn't have to warn indiscriminently, but +// could check if the source value actually doesn't fit into the target type +// and only warn in those cases. +#if defined(__GNUC__) && __GNUC__ > 6 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wsign-conversion" +#endif + +namespace gsl +{ + +// [views.constants], constants +constexpr const std::ptrdiff_t dynamic_extent = -1; + +template +class span; + +// implementation details +namespace details +{ + template + struct is_span_oracle : std::false_type + { + }; + + template + struct is_span_oracle> : std::true_type + { + }; + + template + struct is_span : public is_span_oracle> + { + }; + + template + struct is_std_array_oracle : std::false_type + { + }; + + template + struct is_std_array_oracle> : std::true_type + { + }; + + template + struct is_std_array : public is_std_array_oracle> + { + }; + + template + struct is_allowed_extent_conversion + : public std::integral_constant + { + }; + + template + struct is_allowed_element_type_conversion + : public std::integral_constant::value> + { + }; + + template + class span_iterator + { + using element_type_ = typename Span::element_type; + + public: +#ifdef _MSC_VER + // Tell Microsoft standard library that span_iterators are checked. + using _Unchecked_type = typename Span::pointer; +#endif + + using iterator_category = std::random_access_iterator_tag; + using value_type = std::remove_cv_t; + using difference_type = typename Span::index_type; + + using reference = std::conditional_t&; + using pointer = std::add_pointer_t; + + span_iterator() = default; + + constexpr span_iterator(const Span* span, typename Span::index_type idx) noexcept + : span_(span), index_(idx) + {} + + friend span_iterator; + template * = nullptr> + constexpr span_iterator(const span_iterator& other) noexcept + : span_iterator(other.span_, other.index_) + {} + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr reference operator*() const + { + Expects(index_ != span_->size()); + return *(span_->data() + index_); + } + + constexpr pointer operator->() const + { + Expects(index_ != span_->size()); + return span_->data() + index_; + } + + constexpr span_iterator& operator++() + { + Expects(0 <= index_ && index_ != span_->size()); + ++index_; + return *this; + } + + constexpr span_iterator operator++(int) + { + auto ret = *this; + ++(*this); + return ret; + } + + constexpr span_iterator& operator--() + { + Expects(index_ != 0 && index_ <= span_->size()); + --index_; + return *this; + } + + constexpr span_iterator operator--(int) + { + auto ret = *this; + --(*this); + return ret; + } + + constexpr span_iterator operator+(difference_type n) const + { + auto ret = *this; + return ret += n; + } + + friend constexpr span_iterator operator+(difference_type n, span_iterator const& rhs) + { + return rhs + n; + } + + constexpr span_iterator& operator+=(difference_type n) + { + Expects((index_ + n) >= 0 && (index_ + n) <= span_->size()); + index_ += n; + return *this; + } + + constexpr span_iterator operator-(difference_type n) const + { + auto ret = *this; + return ret -= n; + } + + constexpr span_iterator& operator-=(difference_type n) { return *this += -n; } + + constexpr difference_type operator-(span_iterator rhs) const + { + Expects(span_ == rhs.span_); + return index_ - rhs.index_; + } + + constexpr reference operator[](difference_type n) const { return *(*this + n); } + + constexpr friend bool operator==(span_iterator lhs, span_iterator rhs) noexcept + { + return lhs.span_ == rhs.span_ && lhs.index_ == rhs.index_; + } + + constexpr friend bool operator!=(span_iterator lhs, span_iterator rhs) noexcept + { + return !(lhs == rhs); + } + + constexpr friend bool operator<(span_iterator lhs, span_iterator rhs) noexcept + { + return lhs.index_ < rhs.index_; + } + + constexpr friend bool operator<=(span_iterator lhs, span_iterator rhs) noexcept + { + return !(rhs < lhs); + } + + constexpr friend bool operator>(span_iterator lhs, span_iterator rhs) noexcept + { + return rhs < lhs; + } + + constexpr friend bool operator>=(span_iterator lhs, span_iterator rhs) noexcept + { + return !(rhs > lhs); + } + +#ifdef _MSC_VER + // MSVC++ iterator debugging support; allows STL algorithms in 15.8+ + // to unwrap span_iterator to a pointer type after a range check in STL + // algorithm calls + friend constexpr void _Verify_range(span_iterator lhs, span_iterator rhs) noexcept + { // test that [lhs, rhs) forms a valid range inside an STL algorithm + Expects(lhs.span_ == rhs.span_ // range spans have to match + && lhs.index_ <= rhs.index_); // range must not be transposed + } + + constexpr void _Verify_offset(const difference_type n) const noexcept + { // test that the iterator *this + n is a valid range in an STL + // algorithm call + Expects((index_ + n) >= 0 && (index_ + n) <= span_->size()); + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr pointer _Unwrapped() const noexcept + { // after seeking *this to a high water mark, or using one of the + // _Verify_xxx functions above, unwrap this span_iterator to a raw + // pointer + return span_->data() + index_; + } + + // Tell the STL that span_iterator should not be unwrapped if it can't + // validate in advance, even in release / optimized builds: +#if defined(GSL_USE_STATIC_CONSTEXPR_WORKAROUND) + static constexpr const bool _Unwrap_when_unverified = false; +#else + static constexpr bool _Unwrap_when_unverified = false; +#endif + GSL_SUPPRESS(con.3) // NO-FORMAT: attribute // TODO: false positive + constexpr void _Seek_to(const pointer p) noexcept + { // adjust the position of *this to previously verified location p + // after _Unwrapped + index_ = p - span_->data(); + } +#endif + + protected: + const Span* span_ = nullptr; + std::ptrdiff_t index_ = 0; + }; + + template + class extent_type + { + public: + using index_type = std::ptrdiff_t; + + static_assert(Ext >= 0, "A fixed-size span must be >= 0 in size."); + + constexpr extent_type() noexcept {} + + template + constexpr extent_type(extent_type ext) + { + static_assert(Other == Ext || Other == dynamic_extent, + "Mismatch between fixed-size extent and size of initializing data."); + Expects(ext.size() == Ext); + } + + constexpr extent_type(index_type size) { Expects(size == Ext); } + + constexpr index_type size() const noexcept { return Ext; } + }; + + template <> + class extent_type + { + public: + using index_type = std::ptrdiff_t; + + template + explicit constexpr extent_type(extent_type ext) : size_(ext.size()) + {} + + explicit constexpr extent_type(index_type size) : size_(size) { Expects(size >= 0); } + + constexpr index_type size() const noexcept { return size_; } + + private: + index_type size_; + }; + + template + struct calculate_subspan_type + { + using type = span; + }; +} // namespace details + +// [span], class template span +template +class span +{ +public: + // constants and types + using element_type = ElementType; + using value_type = std::remove_cv_t; + using index_type = std::ptrdiff_t; + using pointer = element_type*; + using reference = element_type&; + + using iterator = details::span_iterator, false>; + using const_iterator = details::span_iterator, true>; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + using size_type = index_type; + +#if defined(GSL_USE_STATIC_CONSTEXPR_WORKAROUND) + static constexpr const index_type extent{Extent}; +#else + static constexpr index_type extent{Extent}; +#endif + + // [span.cons], span constructors, copy, assignment, and destructor + template " SFINAE, + // since "std::enable_if_t" is ill-formed when Extent is greater than 0. + class = std::enable_if_t<(Dependent || Extent <= 0)>> + constexpr span() noexcept : storage_(nullptr, details::extent_type<0>()) + {} + + constexpr span(pointer ptr, index_type count) : storage_(ptr, count) {} + + constexpr span(pointer firstElem, pointer lastElem) + : storage_(firstElem, std::distance(firstElem, lastElem)) + {} + + template + constexpr span(element_type (&arr)[N]) noexcept + : storage_(KnownNotNull{std::addressof(arr[0])}, details::extent_type()) + {} + + template 0)>> + constexpr span(std::array, N>& arr) noexcept + : storage_(KnownNotNull{arr.data()}, details::extent_type()) + { + } + + constexpr span(std::array, 0>&) noexcept + : storage_(static_cast(nullptr), details::extent_type<0>()) + { + } + + template 0)>> + constexpr span(const std::array, N>& arr) noexcept + : storage_(KnownNotNull{arr.data()}, details::extent_type()) + { + } + + constexpr span(const std::array, 0>&) noexcept + : storage_(static_cast(nullptr), details::extent_type<0>()) + { + } + + // NB: the SFINAE here uses .data() as a incomplete/imperfect proxy for the requirement + // on Container to be a contiguous sequence container. + template ::value && !details::is_std_array::value && + std::is_convertible::value && + std::is_convertible().data())>::value>> + constexpr span(Container& cont) : span(cont.data(), narrow(cont.size())) + {} + + template ::value && !details::is_span::value && + std::is_convertible::value && + std::is_convertible().data())>::value>> + constexpr span(const Container& cont) : span(cont.data(), narrow(cont.size())) + {} + + constexpr span(const span& other) noexcept = default; + + template < + class OtherElementType, std::ptrdiff_t OtherExtent, + class = std::enable_if_t< + details::is_allowed_extent_conversion::value && + details::is_allowed_element_type_conversion::value>> + constexpr span(const span& other) + : storage_(other.data(), details::extent_type(other.size())) + {} + + ~span() noexcept = default; + constexpr span& operator=(const span& other) noexcept = default; + + // [span.sub], span subviews + template + constexpr span first() const + { + Expects(Count >= 0 && Count <= size()); + return {data(), Count}; + } + + template + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr span last() const + { + Expects(Count >= 0 && size() - Count >= 0); + return {data() + (size() - Count), Count}; + } + + template + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr auto subspan() const -> + typename details::calculate_subspan_type::type + { + Expects((Offset >= 0 && size() - Offset >= 0) && + (Count == dynamic_extent || (Count >= 0 && Offset + Count <= size()))); + + return {data() + Offset, Count == dynamic_extent ? size() - Offset : Count}; + } + + constexpr span first(index_type count) const + { + Expects(count >= 0 && count <= size()); + return {data(), count}; + } + + constexpr span last(index_type count) const + { + return make_subspan(size() - count, dynamic_extent, subspan_selector{}); + } + + constexpr span subspan(index_type offset, + index_type count = dynamic_extent) const + { + return make_subspan(offset, count, subspan_selector{}); + } + + // [span.obs], span observers + constexpr index_type size() const noexcept { return storage_.size(); } + constexpr index_type size_bytes() const noexcept + { + return size() * narrow_cast(sizeof(element_type)); + } + constexpr bool empty() const noexcept { return size() == 0; } + + // [span.elem], span element access + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + constexpr reference operator[](index_type idx) const + { + Expects(CheckRange(idx, storage_.size())); + return data()[idx]; + } + + constexpr reference at(index_type idx) const { return this->operator[](idx); } + constexpr reference operator()(index_type idx) const { return this->operator[](idx); } + constexpr pointer data() const noexcept { return storage_.data(); } + + // [span.iter], span iterator support + constexpr iterator begin() const noexcept { return {this, 0}; } + constexpr iterator end() const noexcept { return {this, size()}; } + + constexpr const_iterator cbegin() const noexcept { return {this, 0}; } + constexpr const_iterator cend() const noexcept { return {this, size()}; } + + constexpr reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; } + constexpr reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; } + + constexpr const_reverse_iterator crbegin() const noexcept + { + return const_reverse_iterator{cend()}; + } + constexpr const_reverse_iterator crend() const noexcept + { + return const_reverse_iterator{cbegin()}; + } + +#ifdef _MSC_VER + // Tell MSVC how to unwrap spans in range-based-for + constexpr pointer _Unchecked_begin() const noexcept { return data(); } + constexpr pointer _Unchecked_end() const noexcept + { + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + return data() + size(); + } +#endif // _MSC_VER + +private: + static constexpr bool CheckRange(index_type idx, index_type size) noexcept + { + // Optimization: + // + // idx >= 0 && idx < size + // => + // static_cast(idx) < static_cast(size) + // + // because size >=0 by span construction, and negative idx will + // wrap around to a value always greater than size when casted. + + // check if we have enough space to wrap around +#if defined(__cpp_if_constexpr) + if constexpr (sizeof(index_type) <= sizeof(size_t)) +#else + if (sizeof(index_type) <= sizeof(size_t)) +#endif + { + return narrow_cast(idx) < narrow_cast(size); + } + else + { + return idx >= 0 && idx < size; + } + } + + // Needed to remove unnecessary null check in subspans + struct KnownNotNull + { + pointer p; + }; + + // this implementation detail class lets us take advantage of the + // empty base class optimization to pay for only storage of a single + // pointer in the case of fixed-size spans + template + class storage_type : public ExtentType + { + public: + // KnownNotNull parameter is needed to remove unnecessary null check + // in subspans and constructors from arrays + template + constexpr storage_type(KnownNotNull data, OtherExtentType ext) + : ExtentType(ext), data_(data.p) + { + Expects(ExtentType::size() >= 0); + } + + template + constexpr storage_type(pointer data, OtherExtentType ext) : ExtentType(ext), data_(data) + { + Expects(ExtentType::size() >= 0); + Expects(data || ExtentType::size() == 0); + } + + constexpr pointer data() const noexcept { return data_; } + + private: + pointer data_; + }; + + storage_type> storage_; + + // The rest is needed to remove unnecessary null check + // in subspans and constructors from arrays + constexpr span(KnownNotNull ptr, index_type count) : storage_(ptr, count) {} + + template + class subspan_selector + { + }; + + template + span make_subspan(index_type offset, index_type count, + subspan_selector) const + { + const span tmp(*this); + return tmp.subspan(offset, count); + } + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute + span make_subspan(index_type offset, index_type count, + subspan_selector) const + { + Expects(offset >= 0 && size() - offset >= 0); + + if (count == dynamic_extent) { return {KnownNotNull{data() + offset}, size() - offset}; } + + Expects(count >= 0 && size() - offset >= count); + return {KnownNotNull{data() + offset}, count}; + } +}; + +#if defined(GSL_USE_STATIC_CONSTEXPR_WORKAROUND) +template +constexpr const typename span::index_type span::extent; +#endif + +// [span.comparison], span comparison operators +template +constexpr bool operator==(span l, span r) +{ + return std::equal(l.begin(), l.end(), r.begin(), r.end()); +} + +template +constexpr bool operator!=(span l, span r) +{ + return !(l == r); +} + +template +constexpr bool operator<(span l, span r) +{ + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); +} + +template +constexpr bool operator<=(span l, span r) +{ + return !(l > r); +} + +template +constexpr bool operator>(span l, span r) +{ + return r < l; +} + +template +constexpr bool operator>=(span l, span r) +{ + return !(l < r); +} + +namespace details +{ + // if we only supported compilers with good constexpr support then + // this pair of classes could collapse down to a constexpr function + + // we should use a narrow_cast<> to go to std::size_t, but older compilers may not see it as + // constexpr + // and so will fail compilation of the template + template + struct calculate_byte_size + : std::integral_constant(sizeof(ElementType) * + static_cast(Extent))> + { + }; + + template + struct calculate_byte_size + : std::integral_constant + { + }; +} // namespace details + +// [span.objectrep], views of object representation +template +span::value> +as_bytes(span s) noexcept +{ + GSL_SUPPRESS(type.1) // NO-FORMAT: attribute + return {reinterpret_cast(s.data()), s.size_bytes()}; +} + +template ::value>> +span::value> +as_writeable_bytes(span s) noexcept +{ + GSL_SUPPRESS(type.1) // NO-FORMAT: attribute + return {reinterpret_cast(s.data()), s.size_bytes()}; +} + +// +// make_span() - Utility functions for creating spans +// +template +constexpr span make_span(ElementType* ptr, + typename span::index_type count) +{ + return span(ptr, count); +} + +template +constexpr span make_span(ElementType* firstElem, ElementType* lastElem) +{ + return span(firstElem, lastElem); +} + +template +constexpr span make_span(ElementType (&arr)[N]) noexcept +{ + return span(arr); +} + +template +constexpr span make_span(Container& cont) +{ + return span(cont); +} + +template +constexpr span make_span(const Container& cont) +{ + return span(cont); +} + +template +constexpr span make_span(Ptr& cont, std::ptrdiff_t count) +{ + return span(cont, count); +} + +template +constexpr span make_span(Ptr& cont) +{ + return span(cont); +} + +// Specialization of gsl::at for span +template +constexpr ElementType& at(span s, index i) +{ + // No bounds checking here because it is done in span::operator[] called below + return s[i]; +} + +} // namespace gsl + +#if defined(_MSC_VER) && !defined(__clang__) +#if _MSC_VER < 1910 +#undef constexpr +#pragma pop_macro("constexpr") + +#endif // _MSC_VER < 1910 + +#pragma warning(pop) +#endif // _MSC_VER + +#if defined(__GNUC__) && __GNUC__ > 6 +#pragma GCC diagnostic pop +#endif // __GNUC__ > 6 + +#endif // GSL_SPAN_H diff --git a/src/algorithms/libs/gsl/include/gsl/string_span b/src/algorithms/libs/gsl/include/gsl/string_span new file mode 100644 index 000000000..85112e5d0 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/string_span @@ -0,0 +1,722 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef GSL_STRING_SPAN_H +#define GSL_STRING_SPAN_H + +#include // for Ensures, Expects +#include // for narrow_cast +#include // for operator!=, operator==, dynamic_extent +#include // for not_null + +#include // for equal, lexicographical_compare +#include // for array +#include // for ptrdiff_t, size_t, nullptr_t +#include // for PTRDIFF_MAX +#include +#include // for basic_string, allocator, char_traits +#include // for declval, is_convertible, enable_if_t, add_... + +#if defined(_MSC_VER) && !defined(__clang__) +#pragma warning(push) + +// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. +#pragma warning(disable : 26446) // TODO: bug in parser - attributes and templates +#pragma warning(disable : 26481) // TODO: suppress does not work inside templates sometimes + +#if _MSC_VER < 1910 +#pragma push_macro("constexpr") +#define constexpr /*constexpr*/ + +#endif // _MSC_VER < 1910 +#endif // _MSC_VER + +namespace gsl +{ +// +// czstring and wzstring +// +// These are "tag" typedefs for C-style strings (i.e. null-terminated character arrays) +// that allow static analysis to help find bugs. +// +// There are no additional features/semantics that we can find a way to add inside the +// type system for these types that will not either incur significant runtime costs or +// (sometimes needlessly) break existing programs when introduced. +// + +template +using basic_zstring = CharT*; + +template +using czstring = basic_zstring; + +template +using cwzstring = basic_zstring; + +template +using cu16zstring = basic_zstring; + +template +using cu32zstring = basic_zstring; + +template +using zstring = basic_zstring; + +template +using wzstring = basic_zstring; + +template +using u16zstring = basic_zstring; + +template +using u32zstring = basic_zstring; + +namespace details +{ + template + std::ptrdiff_t string_length(const CharT* str, std::ptrdiff_t n) + { + if (str == nullptr || n <= 0) return 0; + + const span str_span{str, n}; + + std::ptrdiff_t len = 0; + while (len < n && str_span[len]) len++; + + return len; + } +} // namespace details + +// +// ensure_sentinel() +// +// Provides a way to obtain an span from a contiguous sequence +// that ends with a (non-inclusive) sentinel value. +// +// Will fail-fast if sentinel cannot be found before max elements are examined. +// +template +span ensure_sentinel(T* seq, std::ptrdiff_t max = PTRDIFF_MAX) +{ + Ensures(seq != nullptr); + + GSL_SUPPRESS(f.23) // NO-FORMAT: attribute // TODO: false positive // TODO: suppress does not work + auto cur = seq; + Ensures(cur != nullptr); // workaround for removing the warning + + GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute // TODO: suppress does not work + while ((cur - seq) < max && *cur != Sentinel) ++cur; + Ensures(*cur == Sentinel); + return {seq, cur - seq}; +} + +// +// ensure_z - creates a span for a zero terminated strings. +// Will fail fast if a null-terminator cannot be found before +// the limit of size_type. +// +template +span ensure_z(CharT* const& sz, std::ptrdiff_t max = PTRDIFF_MAX) +{ + return ensure_sentinel(sz, max); +} + +template +span ensure_z(CharT (&sz)[N]) +{ + return ensure_z(&sz[0], narrow_cast(N)); +} + +template +span::type, dynamic_extent> +ensure_z(Cont& cont) +{ + return ensure_z(cont.data(), narrow_cast(cont.size())); +} + +template +class basic_string_span; + +namespace details { + template + struct is_basic_string_span_oracle : std::false_type + { + }; + + template + struct is_basic_string_span_oracle> : std::true_type + { + }; + + template + struct is_basic_string_span : is_basic_string_span_oracle> + { + }; +} // namespace details + +// +// string_span and relatives +// +template +class basic_string_span +{ +public: + using element_type = CharT; + using value_type = std::remove_cv_t; + using pointer = std::add_pointer_t; + using reference = std::add_lvalue_reference_t; + using const_reference = std::add_lvalue_reference_t>; + using impl_type = span; + + using index_type = typename impl_type::index_type; + using iterator = typename impl_type::iterator; + using const_iterator = typename impl_type::const_iterator; + using reverse_iterator = typename impl_type::reverse_iterator; + using const_reverse_iterator = typename impl_type::const_reverse_iterator; + + using size_type = index_type; + + // default (empty) + constexpr basic_string_span() noexcept = default; + + // copy + constexpr basic_string_span(const basic_string_span& other) noexcept = default; + + // assign + constexpr basic_string_span& operator=(const basic_string_span& other) noexcept = default; + + constexpr basic_string_span(pointer ptr, index_type length) : span_(ptr, length) {} + constexpr basic_string_span(pointer firstElem, pointer lastElem) : span_(firstElem, lastElem) {} + + // From static arrays - if 0-terminated, remove 0 from the view + // All other containers allow 0s within the length, so we do not remove them + template + constexpr basic_string_span(element_type (&arr)[N]) : span_(remove_z(arr)) + {} + + template > + constexpr basic_string_span(std::array& arr) noexcept : span_(arr) + {} + + template > + constexpr basic_string_span(const std::array& arr) noexcept : span_(arr) + {} + + // Container signature should work for basic_string after C++17 version exists + template + // GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute // TODO: parser bug + constexpr basic_string_span(std::basic_string& str) + : span_(&str[0], narrow_cast(str.length())) + {} + + template + constexpr basic_string_span(const std::basic_string& str) + : span_(&str[0], str.length()) + {} + + // from containers. Containers must have a pointer type and data() function signatures + template ::value && + std::is_convertible::value && + std::is_convertible().data())>::value>> + constexpr basic_string_span(Container& cont) : span_(cont) + {} + + template ::value && + std::is_convertible::value && + std::is_convertible().data())>::value>> + constexpr basic_string_span(const Container& cont) : span_(cont) + {} + + // from string_span + template < + class OtherValueType, std::ptrdiff_t OtherExtent, + class = std::enable_if_t::impl_type, impl_type>::value>> + constexpr basic_string_span(basic_string_span other) + : span_(other.data(), other.length()) + {} + + template + constexpr basic_string_span first() const + { + return {span_.template first()}; + } + + constexpr basic_string_span first(index_type count) const + { + return {span_.first(count)}; + } + + template + constexpr basic_string_span last() const + { + return {span_.template last()}; + } + + constexpr basic_string_span last(index_type count) const + { + return {span_.last(count)}; + } + + template + constexpr basic_string_span subspan() const + { + return {span_.template subspan()}; + } + + constexpr basic_string_span + subspan(index_type offset, index_type count = dynamic_extent) const + { + return {span_.subspan(offset, count)}; + } + + constexpr reference operator[](index_type idx) const { return span_[idx]; } + constexpr reference operator()(index_type idx) const { return span_[idx]; } + + constexpr pointer data() const { return span_.data(); } + + constexpr index_type length() const noexcept { return span_.size(); } + constexpr index_type size() const noexcept { return span_.size(); } + constexpr index_type size_bytes() const noexcept { return span_.size_bytes(); } + constexpr index_type length_bytes() const noexcept { return span_.length_bytes(); } + constexpr bool empty() const noexcept { return size() == 0; } + + constexpr iterator begin() const noexcept { return span_.begin(); } + constexpr iterator end() const noexcept { return span_.end(); } + + constexpr const_iterator cbegin() const noexcept { return span_.cbegin(); } + constexpr const_iterator cend() const noexcept { return span_.cend(); } + + constexpr reverse_iterator rbegin() const noexcept { return span_.rbegin(); } + constexpr reverse_iterator rend() const noexcept { return span_.rend(); } + + constexpr const_reverse_iterator crbegin() const noexcept { return span_.crbegin(); } + constexpr const_reverse_iterator crend() const noexcept { return span_.crend(); } + +private: + static impl_type remove_z(pointer const& sz, std::ptrdiff_t max) + { + return {sz, details::string_length(sz, max)}; + } + + template + static impl_type remove_z(element_type (&sz)[N]) + { + return remove_z(&sz[0], narrow_cast(N)); + } + + impl_type span_; +}; + +template +using string_span = basic_string_span; + +template +using cstring_span = basic_string_span; + +template +using wstring_span = basic_string_span; + +template +using cwstring_span = basic_string_span; + +template +using u16string_span = basic_string_span; + +template +using cu16string_span = basic_string_span; + +template +using u32string_span = basic_string_span; + +template +using cu32string_span = basic_string_span; + +// +// to_string() allow (explicit) conversions from string_span to string +// + +template +std::basic_string::type> +to_string(basic_string_span view) +{ + return {view.data(), narrow_cast(view.length())}; +} + +template , + typename Allocator = std::allocator, typename gCharT, std::ptrdiff_t Extent> +std::basic_string to_basic_string(basic_string_span view) +{ + return {view.data(), narrow_cast(view.length())}; +} + +template +basic_string_span::value> +as_bytes(basic_string_span s) noexcept +{ + GSL_SUPPRESS(type.1) // NO-FORMAT: attribute + return {reinterpret_cast(s.data()), s.size_bytes()}; +} + +template ::value>> +basic_string_span::value> +as_writeable_bytes(basic_string_span s) noexcept +{ + GSL_SUPPRESS(type.1) // NO-FORMAT: attribute + return {reinterpret_cast(s.data()), s.size_bytes()}; +} + +// zero-terminated string span, used to convert +// zero-terminated spans to legacy strings +template +class basic_zstring_span { +public: + using value_type = CharT; + using const_value_type = std::add_const_t; + + using pointer = std::add_pointer_t; + using const_pointer = std::add_pointer_t; + + using zstring_type = basic_zstring; + using const_zstring_type = basic_zstring; + + using impl_type = span; + using string_span_type = basic_string_span; + + constexpr basic_zstring_span(impl_type s) : span_(s) + { + // expects a zero-terminated span + Expects(s[s.size() - 1] == '\0'); + } + + // copy + constexpr basic_zstring_span(const basic_zstring_span& other) = default; + + // move + constexpr basic_zstring_span(basic_zstring_span&& other) = default; + + // assign + constexpr basic_zstring_span& operator=(const basic_zstring_span& other) = default; + + // move assign + constexpr basic_zstring_span& operator=(basic_zstring_span&& other) = default; + + constexpr bool empty() const noexcept { return span_.size() == 0; } + + constexpr string_span_type as_string_span() const noexcept + { + const auto sz = span_.size(); + return {span_.data(), sz > 1 ? sz - 1 : 0}; + } + constexpr string_span_type ensure_z() const { return gsl::ensure_z(span_); } + + constexpr const_zstring_type assume_z() const noexcept { return span_.data(); } + +private: + impl_type span_; +}; + +template +using zstring_span = basic_zstring_span; + +template +using wzstring_span = basic_zstring_span; + +template +using u16zstring_span = basic_zstring_span; + +template +using u32zstring_span = basic_zstring_span; + +template +using czstring_span = basic_zstring_span; + +template +using cwzstring_span = basic_zstring_span; + +template +using cu16zstring_span = basic_zstring_span; + +template +using cu32zstring_span = basic_zstring_span; + +// operator == +template ::value || + std::is_convertible>>::value>> +bool operator==(const gsl::basic_string_span& one, const T& other) +{ + const gsl::basic_string_span> tmp(other); + return std::equal(one.begin(), one.end(), tmp.begin(), tmp.end()); +} + +template ::value && + std::is_convertible>>::value>> +bool operator==(const T& one, const gsl::basic_string_span& other) +{ + const gsl::basic_string_span> tmp(one); + return std::equal(tmp.begin(), tmp.end(), other.begin(), other.end()); +} + +// operator != +template , Extent>>::value>> +bool operator!=(gsl::basic_string_span one, const T& other) +{ + return !(one == other); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename = std::enable_if_t< + std::is_convertible, Extent>>::value && + !gsl::details::is_basic_string_span::value>> +bool operator!=(const T& one, gsl::basic_string_span other) +{ + return !(one == other); +} + +// operator< +template , Extent>>::value>> +bool operator<(gsl::basic_string_span one, const T& other) +{ + const gsl::basic_string_span, Extent> tmp(other); + return std::lexicographical_compare(one.begin(), one.end(), tmp.begin(), tmp.end()); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename = std::enable_if_t< + std::is_convertible, Extent>>::value && + !gsl::details::is_basic_string_span::value>> +bool operator<(const T& one, gsl::basic_string_span other) +{ + gsl::basic_string_span, Extent> tmp(one); + return std::lexicographical_compare(tmp.begin(), tmp.end(), other.begin(), other.end()); +} + +#ifndef _MSC_VER + +// VS treats temp and const containers as convertible to basic_string_span, +// so the cases below are already covered by the previous operators + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator<(gsl::basic_string_span one, const T& other) +{ + gsl::basic_string_span, Extent> tmp(other); + return std::lexicographical_compare(one.begin(), one.end(), tmp.begin(), tmp.end()); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator<(const T& one, gsl::basic_string_span other) +{ + gsl::basic_string_span, Extent> tmp(one); + return std::lexicographical_compare(tmp.begin(), tmp.end(), other.begin(), other.end()); +} +#endif + +// operator <= +template , Extent>>::value>> +bool operator<=(gsl::basic_string_span one, const T& other) +{ + return !(other < one); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename = std::enable_if_t< + std::is_convertible, Extent>>::value && + !gsl::details::is_basic_string_span::value>> +bool operator<=(const T& one, gsl::basic_string_span other) +{ + return !(other < one); +} + +#ifndef _MSC_VER + +// VS treats temp and const containers as convertible to basic_string_span, +// so the cases below are already covered by the previous operators + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator<=(gsl::basic_string_span one, const T& other) +{ + return !(other < one); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator<=(const T& one, gsl::basic_string_span other) +{ + return !(other < one); +} +#endif + +// operator> +template , Extent>>::value>> +bool operator>(gsl::basic_string_span one, const T& other) +{ + return other < one; +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename = std::enable_if_t< + std::is_convertible, Extent>>::value && + !gsl::details::is_basic_string_span::value>> +bool operator>(const T& one, gsl::basic_string_span other) +{ + return other < one; +} + +#ifndef _MSC_VER + +// VS treats temp and const containers as convertible to basic_string_span, +// so the cases below are already covered by the previous operators + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator>(gsl::basic_string_span one, const T& other) +{ + return other < one; +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator>(const T& one, gsl::basic_string_span other) +{ + return other < one; +} +#endif + +// operator >= +template , Extent>>::value>> +bool operator>=(gsl::basic_string_span one, const T& other) +{ + return !(one < other); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename = std::enable_if_t< + std::is_convertible, Extent>>::value && + !gsl::details::is_basic_string_span::value>> +bool operator>=(const T& one, gsl::basic_string_span other) +{ + return !(one < other); +} + +#ifndef _MSC_VER + +// VS treats temp and const containers as convertible to basic_string_span, +// so the cases below are already covered by the previous operators + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator>=(gsl::basic_string_span one, const T& other) +{ + return !(one < other); +} + +template < + typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, + typename DataType = typename T::value_type, + typename = std::enable_if_t< + !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && + std::is_convertible::value && + std::is_same().size(), *std::declval().data())>, + DataType>::value>> +bool operator>=(const T& one, gsl::basic_string_span other) +{ + return !(one < other); +} +#endif +} // namespace gsl + +#if defined(_MSC_VER) && !defined(__clang__) +#pragma warning(pop) + +#if _MSC_VER < 1910 +#undef constexpr +#pragma pop_macro("constexpr") + +#endif // _MSC_VER < 1910 +#endif // _MSC_VER + +#endif // GSL_STRING_SPAN_H diff --git a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc index 37c3ca36f..505f99681 100644 --- a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc +++ b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc @@ -208,10 +208,9 @@ void signal_generator_c::generate_codes() { if (signal_[sat].at(0) == '5') { - char signal[3]; - strcpy(signal, "5X"); + std::array signal = {{'5', 'X', '\0'}}; - galileo_e5_a_code_gen_complex_sampled(sampled_code_data_[sat], signal, PRN_[sat], fs_in_, + galileo_e5_a_code_gen_complex_sampled(gsl::span(sampled_code_data_[sat], vector_length_), signal, PRN_[sat], fs_in_, static_cast(GALILEO_E5A_CODE_LENGTH_CHIPS) - delay_chips_[sat]); //noise if (noise_flag_) @@ -226,10 +225,9 @@ void signal_generator_c::generate_codes() { // Generate one code-period of E1B signal bool cboc = true; - char signal[3]; - strcpy(signal, "1B"); + std::array signal = {{'1', 'B', '\0'}}; - galileo_e1_code_gen_complex_sampled(code, signal, cboc, PRN_[sat], fs_in_, + galileo_e1_code_gen_complex_sampled(gsl::span(code, 64000), signal, cboc, PRN_[sat], fs_in_, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) - delay_chips_[sat]); // Obtain the desired CN0 assuming that Pn = 1. @@ -251,9 +249,9 @@ void signal_generator_c::generate_codes() // Generate E1C signal (25 code-periods, with secondary code) sampled_code_pilot_[sat] = static_cast(std::malloc(vector_length_ * sizeof(gr_complex))); - strcpy(signal, "1C"); + std::array signal_1C = {{'1', 'C', '\0'}}; - galileo_e1_code_gen_complex_sampled(sampled_code_pilot_[sat], signal, cboc, PRN_[sat], fs_in_, + galileo_e1_code_gen_complex_sampled(gsl::span(sampled_code_pilot_[sat], vector_length_), signal_1C, cboc, PRN_[sat], fs_in_, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) - delay_chips_[sat], true); // Obtain the desired CN0 assuming that Pn = 1. diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index ac916cef3..83d602ab1 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -227,12 +227,12 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( for (uint32_t PRN = 1; PRN <= GALILEO_E1_NUMBER_OF_CODES; PRN++) { - char data_signal[3] = "1B"; + std::array data_signal = {'1', 'B', '\0'}; if (d_track_pilot) { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_sinboc11_float(ca_codes_f, pilot_signal, PRN); - galileo_e1_code_gen_sinboc11_float(data_codes_f, data_signal, PRN); + std::array pilot_signal = {'1', 'C', '\0'}; + galileo_e1_code_gen_sinboc11_float(gsl::span(ca_codes_f, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * code_samples_per_chip), pilot_signal, PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(data_codes_f, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * code_samples_per_chip), data_signal, PRN); // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++) @@ -255,7 +255,7 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( } else { - galileo_e1_code_gen_sinboc11_float(ca_codes_f, data_signal, PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(ca_codes_f, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * code_samples_per_chip), data_signal, PRN); // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++) diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 5f3241696..37a0fb66e 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -217,7 +217,8 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( for (uint32_t PRN = 1; PRN <= GALILEO_E5A_NUMBER_OF_CODES; PRN++) { - galileo_e5_a_code_gen_complex_primary(aux_code, PRN, const_cast(sig_)); + std::array sig_a = {'5', 'X', '\0'}; + galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, code_length_chips * code_samples_per_chip), PRN, sig_a); if (trk_param_fpga.track_pilot) { diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index e359dba88..2d244fbc0 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -212,7 +212,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int32_t), volk_gnsssdr_get_alignment())); for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l1_ca_code_gen_int(&d_ca_codes[(int32_t(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0); + gps_l1_ca_code_gen_int(gsl::span(&d_ca_codes[static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * (PRN - 1)], &d_ca_codes[static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * (PRN)]), PRN, 0); // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t k = 0; k < GPS_L1_CA_CODE_LENGTH_CHIPS; k++) diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index 7b821a9f9..6cf1c2071 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -128,7 +128,7 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l2c_m_code_gen_float(ca_codes_f, PRN); + gps_l2c_m_code_gen_float(gsl::span(ca_codes_f, static_cast(GPS_L2_M_CODE_LENGTH_CHIPS)), PRN); for (unsigned int s = 0; s < 2 * static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); s++) { d_ca_codes[static_cast(GPS_L2_M_CODE_LENGTH_CHIPS) * (PRN - 1) + s] = static_cast(ca_codes_f[s]); diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 1dca0c03a..f212bbb0a 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -236,8 +236,8 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( { if (track_pilot) { - gps_l5q_code_gen_float(tracking_code, PRN); - gps_l5i_code_gen_float(data_code, PRN); + gps_l5q_code_gen_float(gsl::span(tracking_code, code_length_chips), PRN); + gps_l5i_code_gen_float(gsl::span(data_code, code_length_chips), PRN); // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) @@ -261,7 +261,7 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( } else { - gps_l5i_code_gen_float(tracking_code, PRN); + gps_l5i_code_gen_float(gsl::span(tracking_code, code_length_chips), PRN); // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 8554b72f8..dbe9fccca 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -63,7 +63,8 @@ #include // for fill_n #include // for fmod, round, floor #include // for exception -#include // for cout, cerr +#include +#include // for cout, cerr #include #include @@ -587,48 +588,52 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_phase_rate_step_rad = 0.0; d_carr_ph_history.clear(); d_code_ph_history.clear(); + std::array Signal_; + std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); if (systemName == "GPS" and signal_type == "1C") { - gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); } else if (systemName == "GPS" and signal_type == "2S") { - gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l2c_m_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); } else if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) { - gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); - gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); + gps_l5q_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "1B") { if (trk_parameters.track_pilot) { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); - galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + std::array pilot_signal = {{'1', 'C', '\0'}}; + + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "5X") { auto *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); - galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); + std::array signal_type_ = {{'5', 'X', '\0'}}; + galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_); if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); @@ -651,7 +656,7 @@ void dll_pll_veml_tracking::start_tracking() } else if (systemName == "Beidou" and signal_type == "B1") { - beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -691,7 +696,7 @@ void dll_pll_veml_tracking::start_tracking() else if (systemName == "Beidou" and signal_type == "B3") { - beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + beidou_b3i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index 3d4ae0ff5..7479c0923 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -188,10 +188,12 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking() d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + std::array Signal_; + std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); // generate local reference ALWAYS starting at chip 1 (2 samples per chip) - galileo_e1_code_gen_complex_sampled(d_ca_code, - d_acquisition_gnss_synchro->Signal, + galileo_e1_code_gen_complex_sampled(gsl::span(d_ca_code, (2 * GALILEO_E1_B_CODE_LENGTH_CHIPS)), + Signal_, false, d_acquisition_gnss_synchro->PRN, 2 * GALILEO_E1_CODE_CHIP_RATE_HZ, diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index 26db5d57f..2c51c403f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -272,7 +272,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - glonass_l1_ca_code_gen_complex(d_ca_code, 0); + glonass_l1_ca_code_gen_complex(gsl::span(d_ca_code, GLONASS_L1_CA_CODE_LENGTH_CHIPS), 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int32_t n = 0; n < d_n_correlator_taps; n++) diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index 563ee72f2..a055283b3 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -266,7 +266,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - glonass_l1_ca_code_gen_complex(d_ca_code, 0); + glonass_l1_ca_code_gen_complex(gsl::span(d_ca_code, GLONASS_L1_CA_CODE_LENGTH_CHIPS), 0); volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS)); multicorrelator_cpu_16sc.set_local_code_and_taps(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc index 02aa70906..21e2d29c9 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc @@ -226,7 +226,7 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - glonass_l1_ca_code_gen_complex(d_ca_code, 0); + glonass_l1_ca_code_gen_complex(gsl::span(d_ca_code, GLONASS_L1_CA_CODE_LENGTH_CHIPS), 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int32_t n = 0; n < d_n_correlator_taps; n++) diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index e76a5978a..cd2905d41 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -269,7 +269,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - glonass_l2_ca_code_gen_complex(d_ca_code, 0); + glonass_l2_ca_code_gen_complex(gsl::span(d_ca_code, static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS)), 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int32_t n = 0; n < d_n_correlator_taps; n++) diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc index 4014c70d7..d98687699 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc @@ -265,7 +265,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - glonass_l2_ca_code_gen_complex(d_ca_code, 0); + glonass_l2_ca_code_gen_complex(gsl::span(d_ca_code, static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS)), 0); volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS)); multicorrelator_cpu_16sc.set_local_code_and_taps(static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc index 8f4151f6a..ba11fa86c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc @@ -226,7 +226,7 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - glonass_l2_ca_code_gen_complex(d_ca_code, 0); + glonass_l2_ca_code_gen_complex(gsl::span(d_ca_code, static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS)), 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GLONASS_L2_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int32_t n = 0; n < d_n_correlator_taps; n++) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index 0a363cac5..de2d017bc 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -217,7 +217,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_complex(gsl::span(d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS)), d_acquisition_gnss_synchro->PRN, 0); multicorrelator_gpu->set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips, d_n_correlator_taps); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index a07831b7d..e9dbc1f3a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -320,7 +321,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_float(gsl::span(d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float)), d_acquisition_gnss_synchro->PRN, 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int32_t n = 0; n < d_n_correlator_taps; n++) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index f69896a7a..1cbb90d30 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -223,7 +223,7 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::start_tracking() d_carrier_doppler_hz = d_acq_carrier_doppler_hz; // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_complex(gsl::span(d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex)), d_acquisition_gnss_synchro->PRN, 0); multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int32_t n = 0; n < d_n_correlator_taps; n++) diff --git a/src/tests/unit-tests/arithmetic/code_generation_test.cc b/src/tests/unit-tests/arithmetic/code_generation_test.cc index e252771af..60d4a7033 100644 --- a/src/tests/unit-tests/arithmetic/code_generation_test.cc +++ b/src/tests/unit-tests/arithmetic/code_generation_test.cc @@ -33,6 +33,12 @@ #include "gps_sdr_signal_processing.h" #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif TEST(CodeGenerationTest, CodeGenGPSL1Test) @@ -48,7 +54,7 @@ TEST(CodeGenerationTest, CodeGenGPSL1Test) for (int i = 0; i < iterations; i++) { - gps_l1_ca_code_gen_complex(_dest, _prn, _chip_shift); + gps_l1_ca_code_gen_complex(gsl::span>(_dest, 1023), _prn, _chip_shift); } end = std::chrono::system_clock::now(); @@ -77,7 +83,7 @@ TEST(CodeGenerationTest, CodeGenGPSL1SampledTest) for (int i = 0; i < iterations; i++) { - gps_l1_ca_code_gen_complex_sampled(_dest, _prn, _fs, _chip_shift); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(_dest, _samplesPerCode), _prn, _fs, _chip_shift); } end = std::chrono::system_clock::now(); @@ -105,7 +111,7 @@ TEST(CodeGenerationTest, ComplexConjugateTest) for (int i = 0; i < iterations; i++) { - complex_exp_gen_conj(_dest, _f, _fs, _samplesPerCode); + complex_exp_gen_conj(gsl::span>(_dest, _samplesPerCode), _f, _fs); } end = std::chrono::system_clock::now(); diff --git a/src/tests/unit-tests/arithmetic/complex_carrier_test.cc b/src/tests/unit-tests/arithmetic/complex_carrier_test.cc index bd87a3da7..1dd943a7a 100644 --- a/src/tests/unit-tests/arithmetic/complex_carrier_test.cc +++ b/src/tests/unit-tests/arithmetic/complex_carrier_test.cc @@ -34,6 +34,12 @@ #include #include #include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif DEFINE_int32(size_carrier_test, 100000, "Size of the arrays used for complex carrier testing"); @@ -120,7 +126,7 @@ TEST(ComplexCarrierTest, OwnComplexImplementation) std::chrono::time_point start, end; start = std::chrono::system_clock::now(); - complex_exp_gen(output, _f, _fs, static_cast(FLAGS_size_carrier_test)); + complex_exp_gen(gsl::span>(output, static_cast(FLAGS_size_carrier_test)), _f, _fs); end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test_fpga.cc index 5d87e5133..96c0374a6 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test_fpga.cc @@ -259,6 +259,7 @@ public: Gnss_Synchro gnss_synchro_master; std::vector gnss_synchro_vec; size_t item_size; + pthread_mutex_t mutex_obs_test = PTHREAD_MUTEX_INITIALIZER; }; int HybridObservablesTestFpga::configure_generator() @@ -305,11 +306,10 @@ int HybridObservablesTestFpga::generate_signal() } -const size_t TEST_OBS_PAGE_SIZE = 0x10000; -const unsigned int TEST_OBS_TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; - void setup_fpga_switch_obs_test(void) { + const size_t TEST_OBS_PAGE_SIZE = 0x10000; + const unsigned int TEST_OBS_TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; int switch_device_descriptor; // driver descriptor volatile unsigned* switch_map_base; // driver memory map @@ -348,7 +348,7 @@ void setup_fpga_switch_obs_test(void) } -static pthread_mutex_t mutex_obs_test = PTHREAD_MUTEX_INITIALIZER; +//static pthread_mutex_t mutex_obs_test = PTHREAD_MUTEX_INITIALIZER; volatile unsigned int send_samples_start_obs_test = 0; @@ -642,9 +642,9 @@ bool HybridObservablesTestFpga::acquire_signal() { std::cout << "ERROR cannot create DMA Process" << std::endl; } - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&mutex_obs_test); send_samples_start_obs_test = 1; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&mutex_obs_test); pthread_join(thread_DMA, nullptr); send_samples_start_obs_test = 0; @@ -669,9 +669,9 @@ bool HybridObservablesTestFpga::acquire_signal() msg_rx->rx_message = 0; top_block->start(); - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&mutex_obs_test); send_samples_start_obs_test = 1; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&mutex_obs_test); acquisition->reset(); // set active @@ -686,9 +686,9 @@ bool HybridObservablesTestFpga::acquire_signal() // wait for the child DMA process to finish pthread_join(thread_DMA, nullptr); - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&mutex_obs_test); send_samples_start_obs_test = 0; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&mutex_obs_test); // the DMA sends the exact number of samples needed for the acquisition. // however because of the LPF in the GPS L1/Gal E1 acquisition, this calculation is approximate diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc index b4c5f432c..98cbc094d 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc @@ -106,7 +106,7 @@ TEST(CpuMulticorrelatorRealCodesTest, MeasureExecutionTime) //local code resampler on GPU // generate local reference (1 sample per chip) - gps_l1_ca_code_gen_float(d_ca_code, 1, 0); + gps_l1_ca_code_gen_float(gsl::span(d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float)), 1, 0); // generate inut signal std::random_device r; std::default_random_engine e1(r()); diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc index 0f133db98..0f56dbab0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc @@ -104,7 +104,7 @@ TEST(CpuMulticorrelatorTest, MeasureExecutionTime) //local code resampler on GPU // generate local reference (1 sample per chip) - gps_l1_ca_code_gen_complex(d_ca_code, 1, 0); + gps_l1_ca_code_gen_complex(gsl::span(d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex)), 1, 0); // generate inut signal std::random_device r; std::default_random_engine e1(r()); diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc index bf997d7b4..d82fcd3cd 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc @@ -248,6 +248,7 @@ public: std::shared_ptr config; Gnss_Synchro gnss_synchro; size_t item_size; + pthread_mutex_t the_mutex = PTHREAD_MUTEX_INITIALIZER; }; @@ -386,12 +387,10 @@ void TrackingPullInTestFpga::configure_receiver( } -const size_t PAGE_SIZE = 0x10000; -const unsigned int TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; - - void setup_fpga_switch(void) { + const size_t PAGE_SIZE_ = 0x10000; + const unsigned int TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; int switch_device_descriptor; // driver descriptor volatile unsigned* switch_map_base; // driver memory map @@ -400,7 +399,7 @@ void setup_fpga_switch(void) LOG(WARNING) << "Cannot open deviceio" << "/dev/uio1"; } - switch_map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE, + switch_map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE_, PROT_READ | PROT_WRITE, MAP_SHARED, switch_device_descriptor, 0)); if (switch_map_base == reinterpret_cast(-1)) @@ -430,7 +429,7 @@ void setup_fpga_switch(void) } -static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; volatile unsigned int send_samples_start = 0; @@ -728,9 +727,9 @@ bool TrackingPullInTestFpga::acquire_signal(int SV_ID) { std::cout << "ERROR cannot create DMA Process" << std::endl; } - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&the_mutex); send_samples_start = 1; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&the_mutex); pthread_join(thread_DMA, nullptr); send_samples_start = 0; @@ -753,9 +752,9 @@ bool TrackingPullInTestFpga::acquire_signal(int SV_ID) msg_rx->rx_message = 0; top_block->start(); - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&the_mutex); send_samples_start = 1; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&the_mutex); acquisition->reset(); // set active @@ -770,9 +769,9 @@ bool TrackingPullInTestFpga::acquire_signal(int SV_ID) // wait for the child DMA process to finish pthread_join(thread_DMA, nullptr); - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&the_mutex); send_samples_start = 0; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&the_mutex); // the DMA sends the exact number of samples needed for the acquisition. // however because of the LPF in the GPS L1/Gal E1 acquisition, this calculation is approximate @@ -1063,9 +1062,9 @@ TEST_F(TrackingPullInTestFpga, ValidationOfResults) tracking->start_tracking(); - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&the_mutex); send_samples_start = 1; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&the_mutex); top_block->start(); @@ -1077,9 +1076,9 @@ TEST_F(TrackingPullInTestFpga, ValidationOfResults) // reset the HW to launch the pending interrupts acquisition->stop_acquisition(); - pthread_mutex_lock(&mutex); + pthread_mutex_lock(&the_mutex); send_samples_start = 0; - pthread_mutex_unlock(&mutex); + pthread_mutex_unlock(&the_mutex); pull_in_results_v.push_back(msg_rx->rx_message != 3); //save last asynchronous tracking message in order to detect a loss of lock From 9b3fd32f436c2eda76fb9baf1c1efe033941e4f9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 01:58:12 +0200 Subject: [PATCH 05/64] Check if the compiler has the header --- src/algorithms/libs/CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index f48579047..4978a3d78 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -92,7 +92,15 @@ else() target_link_libraries(algorithms_libs PRIVATE Boost::filesystem Boost::system) endif() -if(NOT (CMAKE_CXX_STANDARD VERSION_LESS 20)) +include(CheckCXXSourceCompiles) +check_cxx_source_compiles(" + #include + int main() + { std::span sv; }" + has_span +) + +if(${has_span}) target_compile_definitions(algorithms_libs PUBLIC -DHAS_SPAN=1) else() target_include_directories(algorithms_libs From c79b360fa73bf852b35ef47063692d81d3688410 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 02:40:12 +0200 Subject: [PATCH 06/64] Replace Guidelines Support Library implementation Source: https://github.com/martinmoene/gsl-lite If works with gcc 4.8 (Microsoft's doesn't) --- src/algorithms/libs/gsl/include/gsl/gsl | 26 +- .../libs/gsl/include/gsl/gsl-lite.hpp | 3110 +++++++++++++++++ .../libs/gsl/include/gsl/gsl_algorithm | 61 - .../libs/gsl/include/gsl/gsl_assert | 177 - src/algorithms/libs/gsl/include/gsl/gsl_byte | 203 -- src/algorithms/libs/gsl/include/gsl/gsl_util | 175 - .../libs/gsl/include/gsl/multi_span | 2293 ------------ src/algorithms/libs/gsl/include/gsl/pointers | 294 -- src/algorithms/libs/gsl/include/gsl/span | 793 ----- .../libs/gsl/include/gsl/string_span | 722 ---- 10 files changed, 3122 insertions(+), 4732 deletions(-) create mode 100644 src/algorithms/libs/gsl/include/gsl/gsl-lite.hpp delete mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_algorithm delete mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_assert delete mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_byte delete mode 100644 src/algorithms/libs/gsl/include/gsl/gsl_util delete mode 100644 src/algorithms/libs/gsl/include/gsl/multi_span delete mode 100644 src/algorithms/libs/gsl/include/gsl/pointers delete mode 100644 src/algorithms/libs/gsl/include/gsl/span delete mode 100644 src/algorithms/libs/gsl/include/gsl/string_span diff --git a/src/algorithms/libs/gsl/include/gsl/gsl b/src/algorithms/libs/gsl/include/gsl/gsl index 55862ebdd..d4c466632 100644 --- a/src/algorithms/libs/gsl/include/gsl/gsl +++ b/src/algorithms/libs/gsl/include/gsl/gsl @@ -1,5 +1,8 @@ -/////////////////////////////////////////////////////////////////////////////// // +// gsl-lite is based on GSL: Guidelines Support Library. +// For more information see https://github.com/martinmoene/gsl-lite +// +// Copyright (c) 2015 Martin Moene // Copyright (c) 2015 Microsoft Corporation. All rights reserved. // // This code is licensed under the MIT License (MIT). @@ -11,19 +14,14 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// -#ifndef GSL_GSL_H -#define GSL_GSL_H +// mimic MS include hierarchy -#include // copy -#include // Ensures/Expects -#include // byte -#include // finally()/narrow()/narrow_cast()... -#include // multi_span, strided_span... -#include // owner, not_null -#include // span -#include // zstring, string_span, zstring_builder... +#pragma once -#endif // GSL_GSL_H +#ifndef GSL_GSL_H_INCLUDED +#define GSL_GSL_H_INCLUDED + +#include "gsl-lite.hpp" + +#endif // GSL_GSL_H_INCLUDED diff --git a/src/algorithms/libs/gsl/include/gsl/gsl-lite.hpp b/src/algorithms/libs/gsl/include/gsl/gsl-lite.hpp new file mode 100644 index 000000000..993b167e0 --- /dev/null +++ b/src/algorithms/libs/gsl/include/gsl/gsl-lite.hpp @@ -0,0 +1,3110 @@ +// +// gsl-lite is based on GSL: Guidelines Support Library. +// For more information see https://github.com/martinmoene/gsl-lite +// +// Copyright (c) 2015-2018 Martin Moene +// Copyright (c) 2015-2018 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#pragma once + +#ifndef GSL_GSL_LITE_HPP_INCLUDED +#define GSL_GSL_LITE_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define gsl_lite_MAJOR 0 +#define gsl_lite_MINOR 34 +#define gsl_lite_PATCH 0 + +#define gsl_lite_VERSION gsl_STRINGIFY(gsl_lite_MAJOR) "." gsl_STRINGIFY(gsl_lite_MINOR) "." gsl_STRINGIFY(gsl_lite_PATCH) + +// gsl-lite backward compatibility: + +#ifdef gsl_CONFIG_ALLOWS_SPAN_CONTAINER_CTOR +#define gsl_CONFIG_ALLOWS_UNCONSTRAINED_SPAN_CONTAINER_CTOR gsl_CONFIG_ALLOWS_SPAN_CONTAINER_CTOR +#pragma message("gsl_CONFIG_ALLOWS_SPAN_CONTAINER_CTOR is deprecated since gsl-lite 0.7.0; replace with gsl_CONFIG_ALLOWS_UNCONSTRAINED_SPAN_CONTAINER_CTOR, or consider span(with_container, cont).") +#endif + +// M-GSL compatibility: + +#if defined(GSL_THROW_ON_CONTRACT_VIOLATION) +#define gsl_CONFIG_CONTRACT_VIOLATION_THROWS 1 +#endif + +#if defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) +#define gsl_CONFIG_CONTRACT_VIOLATION_TERMINATES 1 +#endif + +#if defined(GSL_UNENFORCED_ON_CONTRACT_VIOLATION) +#define gsl_CONFIG_CONTRACT_LEVEL_OFF 1 +#endif + +// Configuration: Features + +#ifndef gsl_FEATURE_WITH_CONTAINER_TO_STD +#define gsl_FEATURE_WITH_CONTAINER_TO_STD 99 +#endif + +#ifndef gsl_FEATURE_MAKE_SPAN_TO_STD +#define gsl_FEATURE_MAKE_SPAN_TO_STD 99 +#endif + +#ifndef gsl_FEATURE_BYTE_SPAN_TO_STD +#define gsl_FEATURE_BYTE_SPAN_TO_STD 99 +#endif + +#ifndef gsl_FEATURE_IMPLICIT_MACRO +#define gsl_FEATURE_IMPLICIT_MACRO 0 +#endif + +#ifndef gsl_FEATURE_OWNER_MACRO +#define gsl_FEATURE_OWNER_MACRO 1 +#endif + +#ifndef gsl_FEATURE_EXPERIMENTAL_RETURN_GUARD +#define gsl_FEATURE_EXPERIMENTAL_RETURN_GUARD 0 +#endif + +// Configuration: Other + +#ifndef gsl_CONFIG_DEPRECATE_TO_LEVEL +#define gsl_CONFIG_DEPRECATE_TO_LEVEL 0 +#endif + +#ifndef gsl_CONFIG_SPAN_INDEX_TYPE +#define gsl_CONFIG_SPAN_INDEX_TYPE size_t +#endif + +#ifndef gsl_CONFIG_NOT_NULL_EXPLICIT_CTOR +#define gsl_CONFIG_NOT_NULL_EXPLICIT_CTOR 0 +#endif + +#ifndef gsl_CONFIG_NOT_NULL_GET_BY_CONST_REF +#define gsl_CONFIG_NOT_NULL_GET_BY_CONST_REF 0 +#endif + +#ifndef gsl_CONFIG_CONFIRMS_COMPILATION_ERRORS +#define gsl_CONFIG_CONFIRMS_COMPILATION_ERRORS 0 +#endif + +#ifndef gsl_CONFIG_ALLOWS_NONSTRICT_SPAN_COMPARISON +#define gsl_CONFIG_ALLOWS_NONSTRICT_SPAN_COMPARISON 1 +#endif + +#ifndef gsl_CONFIG_ALLOWS_UNCONSTRAINED_SPAN_CONTAINER_CTOR +#define gsl_CONFIG_ALLOWS_UNCONSTRAINED_SPAN_CONTAINER_CTOR 0 +#endif + +#if defined(gsl_CONFIG_CONTRACT_LEVEL_ON) +#define gsl_CONFIG_CONTRACT_LEVEL_MASK 0x11 +#elif defined(gsl_CONFIG_CONTRACT_LEVEL_OFF) +#define gsl_CONFIG_CONTRACT_LEVEL_MASK 0x00 +#elif defined(gsl_CONFIG_CONTRACT_LEVEL_EXPECTS_ONLY) +#define gsl_CONFIG_CONTRACT_LEVEL_MASK 0x01 +#elif defined(gsl_CONFIG_CONTRACT_LEVEL_ENSURES_ONLY) +#define gsl_CONFIG_CONTRACT_LEVEL_MASK 0x10 +#else +#define gsl_CONFIG_CONTRACT_LEVEL_MASK 0x11 +#endif + +#if 2 <= defined(gsl_CONFIG_CONTRACT_VIOLATION_THROWS) + defined(gsl_CONFIG_CONTRACT_VIOLATION_TERMINATES) + defined(gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER) +#error only one of gsl_CONFIG_CONTRACT_VIOLATION_THROWS, gsl_CONFIG_CONTRACT_VIOLATION_TERMINATES and gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER may be defined. +#elif defined(gsl_CONFIG_CONTRACT_VIOLATION_THROWS) +#define gsl_CONFIG_CONTRACT_VIOLATION_THROWS_V 1 +#define gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER_V 0 +#elif defined(gsl_CONFIG_CONTRACT_VIOLATION_TERMINATES) +#define gsl_CONFIG_CONTRACT_VIOLATION_THROWS_V 0 +#define gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER_V 0 +#elif defined(gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER) +#define gsl_CONFIG_CONTRACT_VIOLATION_THROWS_V 0 +#define gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER_V 1 +#else +#define gsl_CONFIG_CONTRACT_VIOLATION_THROWS_V 0 +#define gsl_CONFIG_CONTRACT_VIOLATION_CALLS_HANDLER_V 0 +#endif + +// C++ language version detection (C++20 is speculative): +// Note: VC14.0/1900 (VS2015) lacks too much from C++14. + +#ifndef gsl_CPLUSPLUS +#if defined(_MSVC_LANG) && !defined(__clang__) +#define gsl_CPLUSPLUS (_MSC_VER == 1900 ? 201103L : _MSVC_LANG) +#else +#define gsl_CPLUSPLUS __cplusplus +#endif +#endif + +#define gsl_CPP98_OR_GREATER (gsl_CPLUSPLUS >= 199711L) +#define gsl_CPP11_OR_GREATER (gsl_CPLUSPLUS >= 201103L) +#define gsl_CPP14_OR_GREATER (gsl_CPLUSPLUS >= 201402L) +#define gsl_CPP17_OR_GREATER (gsl_CPLUSPLUS >= 201703L) +#define gsl_CPP20_OR_GREATER (gsl_CPLUSPLUS >= 202000L) + +// C++ language version (represent 98 as 3): + +#define gsl_CPLUSPLUS_V (gsl_CPLUSPLUS / 100 - (gsl_CPLUSPLUS > 200000 ? 2000 : 1994)) + +// half-open range [lo..hi): +#define gsl_BETWEEN(v, lo, hi) ((lo) <= (v) && (v) < (hi)) + +// Compiler versions: +// +// MSVC++ 6.0 _MSC_VER == 1200 (Visual Studio 6.0) +// MSVC++ 7.0 _MSC_VER == 1300 (Visual Studio .NET 2002) +// MSVC++ 7.1 _MSC_VER == 1310 (Visual Studio .NET 2003) +// MSVC++ 8.0 _MSC_VER == 1400 (Visual Studio 2005) +// MSVC++ 9.0 _MSC_VER == 1500 (Visual Studio 2008) +// MSVC++ 10.0 _MSC_VER == 1600 (Visual Studio 2010) +// MSVC++ 11.0 _MSC_VER == 1700 (Visual Studio 2012) +// MSVC++ 12.0 _MSC_VER == 1800 (Visual Studio 2013) +// MSVC++ 14.0 _MSC_VER == 1900 (Visual Studio 2015) +// MSVC++ 14.1 _MSC_VER >= 1910 (Visual Studio 2017) + +#if defined(_MSC_VER) && !defined(__clang__) +#define gsl_COMPILER_MSVC_VER (_MSC_VER) +#define gsl_COMPILER_MSVC_VERSION (_MSC_VER / 10 - 10 * (5 + (_MSC_VER < 1900))) +#else +#define gsl_COMPILER_MSVC_VER 0 +#define gsl_COMPILER_MSVC_VERSION 0 +#endif + +#define gsl_COMPILER_VERSION(major, minor, patch) (10 * (10 * (major) + (minor)) + (patch)) + +#if defined(__clang__) +#define gsl_COMPILER_CLANG_VERSION gsl_COMPILER_VERSION(__clang_major__, __clang_minor__, __clang_patchlevel__) +#else +#define gsl_COMPILER_CLANG_VERSION 0 +#endif + +#if defined(__GNUC__) && !defined(__clang__) +#define gsl_COMPILER_GNUC_VERSION gsl_COMPILER_VERSION(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__) +#else +#define gsl_COMPILER_GNUC_VERSION 0 +#endif + +// Method enabling (C++98, VC120 (VS2013) cannot use __VA_ARGS__) + +#define gsl_REQUIRES_0(VA) \ + template ::type = 0> + +#define gsl_REQUIRES_T(VA) \ + , typename = typename std::enable_if<(VA), gsl::detail::enabler>::type + +#define gsl_REQUIRES_R(R, VA) \ + typename std::enable_if::type + +#define gsl_REQUIRES_A(VA) \ + , typename std::enable_if::type = nullptr + +// Compiler non-strict aliasing: + +#if defined(__clang__) || defined(__GNUC__) +#define gsl_may_alias __attribute__((__may_alias__)) +#else +#define gsl_may_alias +#endif + +// Presence of gsl, language and library features: + +#define gsl_IN_STD(v) (((v) == 98 ? 3 : (v)) >= gsl_CPLUSPLUS_V) + +#define gsl_DEPRECATE_TO_LEVEL(level) (level <= gsl_CONFIG_DEPRECATE_TO_LEVEL) +#define gsl_FEATURE_TO_STD(feature) (gsl_IN_STD(gsl_FEATURE(feature##_TO_STD))) +#define gsl_FEATURE(feature) (gsl_FEATURE_##feature) +#define gsl_CONFIG(feature) (gsl_CONFIG_##feature) +#define gsl_HAVE(feature) (gsl_HAVE_##feature) + +// Presence of wide character support: + +#ifdef __DJGPP__ +#define gsl_HAVE_WCHAR 0 +#else +#define gsl_HAVE_WCHAR 1 +#endif + +// Presence of language & library features: + +#ifdef _HAS_CPP0X +#define gsl_HAS_CPP0X _HAS_CPP0X +#else +#define gsl_HAS_CPP0X 0 +#endif + +#define gsl_CPP11_100 (gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1600) +#define gsl_CPP11_110 (gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1700) +#define gsl_CPP11_120 (gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1800) +#define gsl_CPP11_140 (gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1900) + +#define gsl_CPP14_000 (gsl_CPP14_OR_GREATER) +#define gsl_CPP14_120 (gsl_CPP14_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1800) +#define gsl_CPP14_140 (gsl_CPP14_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1900) + +#define gsl_CPP17_000 (gsl_CPP17_OR_GREATER) +#define gsl_CPP17_140 (gsl_CPP17_OR_GREATER || gsl_COMPILER_MSVC_VER >= 1900) + +#define gsl_CPP11_140_CPP0X_90 (gsl_CPP11_140 || (gsl_COMPILER_MSVC_VER >= 1500 && gsl_HAS_CPP0X)) +#define gsl_CPP11_140_CPP0X_100 (gsl_CPP11_140 || (gsl_COMPILER_MSVC_VER >= 1600 && gsl_HAS_CPP0X)) + +// Presence of C++11 language features: + +#define gsl_HAVE_AUTO gsl_CPP11_100 +#define gsl_HAVE_NULLPTR gsl_CPP11_100 +#define gsl_HAVE_RVALUE_REFERENCE gsl_CPP11_100 + +#define gsl_HAVE_ENUM_CLASS gsl_CPP11_110 + +#define gsl_HAVE_ALIAS_TEMPLATE gsl_CPP11_120 +#define gsl_HAVE_DEFAULT_FUNCTION_TEMPLATE_ARG gsl_CPP11_120 +#define gsl_HAVE_EXPLICIT gsl_CPP11_120 +#define gsl_HAVE_INITIALIZER_LIST gsl_CPP11_120 + +#define gsl_HAVE_CONSTEXPR_11 gsl_CPP11_140 +#define gsl_HAVE_IS_DEFAULT gsl_CPP11_140 +#define gsl_HAVE_IS_DELETE gsl_CPP11_140 +#define gsl_HAVE_NOEXCEPT gsl_CPP11_140 + +#if gsl_CPP11_OR_GREATER +// see above +#endif + +// Presence of C++14 language features: + +#define gsl_HAVE_CONSTEXPR_14 gsl_CPP14_000 +#define gsl_HAVE_DECLTYPE_AUTO gsl_CPP14_140 + +// Presence of C++17 language features: +// MSVC: template parameter deduction guides since Visual Studio 2017 v15.7 + +#define gsl_HAVE_ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE gsl_CPP17_000 +#define gsl_HAVE_DEDUCTION_GUIDES (gsl_CPP17_000 && !gsl_BETWEEN(gsl_COMPILER_MSVC_VERSION, 1, 999)) + +// Presence of C++ library features: + +#define gsl_HAVE_ADDRESSOF gsl_CPP17_000 +#define gsl_HAVE_ARRAY gsl_CPP11_110 +#define gsl_HAVE_TYPE_TRAITS gsl_CPP11_110 +#define gsl_HAVE_TR1_TYPE_TRAITS gsl_CPP11_110 + +#define gsl_HAVE_CONTAINER_DATA_METHOD gsl_CPP11_140_CPP0X_90 +#define gsl_HAVE_STD_DATA gsl_CPP17_000 + +#define gsl_HAVE_SIZED_TYPES gsl_CPP11_140 + +#define gsl_HAVE_MAKE_SHARED gsl_CPP11_140_CPP0X_100 +#define gsl_HAVE_SHARED_PTR gsl_CPP11_140_CPP0X_100 +#define gsl_HAVE_UNIQUE_PTR gsl_CPP11_140_CPP0X_100 + +#define gsl_HAVE_MAKE_UNIQUE gsl_CPP14_120 + +#define gsl_HAVE_UNCAUGHT_EXCEPTIONS gsl_CPP17_140 + +#define gsl_HAVE_ADD_CONST gsl_HAVE_TYPE_TRAITS +#define gsl_HAVE_INTEGRAL_CONSTANT gsl_HAVE_TYPE_TRAITS +#define gsl_HAVE_REMOVE_CONST gsl_HAVE_TYPE_TRAITS +#define gsl_HAVE_REMOVE_REFERENCE gsl_HAVE_TYPE_TRAITS + +#define gsl_HAVE_TR1_ADD_CONST gsl_HAVE_TR1_TYPE_TRAITS +#define gsl_HAVE_TR1_INTEGRAL_CONSTANT gsl_HAVE_TR1_TYPE_TRAITS +#define gsl_HAVE_TR1_REMOVE_CONST gsl_HAVE_TR1_TYPE_TRAITS +#define gsl_HAVE_TR1_REMOVE_REFERENCE gsl_HAVE_TR1_TYPE_TRAITS + +// C++ feature usage: + +#if gsl_HAVE(ADDRESSOF) +#define gsl_ADDRESSOF(x) std::addressof(x) +#else +#define gsl_ADDRESSOF(x) (&x) +#endif + +#if gsl_HAVE(CONSTEXPR_11) +#define gsl_constexpr constexpr +#else +#define gsl_constexpr /*constexpr*/ +#endif + +#if gsl_HAVE(CONSTEXPR_14) +#define gsl_constexpr14 constexpr +#else +#define gsl_constexpr14 /*constexpr*/ +#endif + +#if gsl_HAVE(EXPLICIT) +#define gsl_explicit explicit +#else +#define gsl_explicit /*explicit*/ +#endif + +#if gsl_FEATURE(IMPLICIT_MACRO) +#define implicit /*implicit*/ +#endif + +#if gsl_HAVE(IS_DELETE) +#define gsl_is_delete = delete +#else +#define gsl_is_delete +#endif + +#if gsl_HAVE(IS_DELETE) +#define gsl_is_delete_access public +#else +#define gsl_is_delete_access private +#endif + +#if !gsl_HAVE(NOEXCEPT) || gsl_CONFIG(CONTRACT_VIOLATION_THROWS_V) +#define gsl_noexcept /*noexcept*/ +#else +#define gsl_noexcept noexcept +#endif + +#if gsl_HAVE(NULLPTR) +#define gsl_nullptr nullptr +#else +#define gsl_nullptr NULL +#endif + +#define gsl_DIMENSION_OF(a) (sizeof(a) / sizeof(0 [a])) + +// Other features: + +#define gsl_HAVE_CONSTRAINED_SPAN_CONTAINER_CTOR \ + (gsl_HAVE_DEFAULT_FUNCTION_TEMPLATE_ARG && gsl_HAVE_CONTAINER_DATA_METHOD) + +// Note: !defined(__NVCC__) doesn't work with nvcc here: +#define gsl_HAVE_UNCONSTRAINED_SPAN_CONTAINER_CTOR \ + (gsl_CONFIG_ALLOWS_UNCONSTRAINED_SPAN_CONTAINER_CTOR && (__NVCC__ == 0)) + +// GSL API (e.g. for CUDA platform): + +#ifndef gsl_api +#ifdef __CUDACC__ +#define gsl_api __host__ __device__ +#else +#define gsl_api /*gsl_api*/ +#endif +#endif + +// Additional includes: + +#if gsl_HAVE(ARRAY) +#include +#endif + +#if gsl_HAVE(INITIALIZER_LIST) +#include +#endif + +#if gsl_HAVE(TYPE_TRAITS) +#include +#elif gsl_HAVE(TR1_TYPE_TRAITS) +#include +#endif + +#if gsl_HAVE(SIZED_TYPES) +#include +#endif + +// MSVC warning suppression macros: + +#if gsl_COMPILER_MSVC_VERSION >= 140 +#define gsl_SUPPRESS_MSGSL_WARNING(expr) [[gsl::suppress(expr)]] +#define gsl_SUPPRESS_MSVC_WARNING(code, descr) __pragma(warning(suppress \ + : code)) +#define gsl_DISABLE_MSVC_WARNINGS(codes) __pragma(warning(push)) __pragma(warning(disable \ + : codes)) +#define gsl_RESTORE_MSVC_WARNINGS() __pragma(warning(pop)) +#else +#define gsl_SUPPRESS_MSGSL_WARNING(expr) +#define gsl_SUPPRESS_MSVC_WARNING(code, descr) +#define gsl_DISABLE_MSVC_WARNINGS(codes) +#define gsl_RESTORE_MSVC_WARNINGS() +#endif + +// Suppress the following MSVC GSL warnings: +// - C26410: gsl::r.32: the parameter 'ptr' is a reference to const unique pointer, use const T* or const T& instead +// - C26415: gsl::r.30: smart pointer parameter 'ptr' is used only to access contained pointer. Use T* or T& instead +// - C26418: gsl::r.36: shared pointer parameter 'ptr' is not copied or moved. Use T* or T& instead +// - C26472, gsl::t.1 : don't use a static_cast for arithmetic conversions; +// use brace initialization, gsl::narrow_cast or gsl::narow +// - C26439, gsl::f.6 : special function 'function' can be declared 'noexcept' +// - C26440, gsl::f.6 : function 'function' can be declared 'noexcept' +// - C26473: gsl::t.1 : don't cast between pointer types where the source type and the target type are the same +// - C26481: gsl::b.1 : don't use pointer arithmetic. Use span instead +// - C26482, gsl::b.2 : only index into arrays using constant expressions +// - C26490: gsl::t.1 : don't use reinterpret_cast + +gsl_DISABLE_MSVC_WARNINGS(26410 26415 26418 26472 26439 26440 26473 26481 26482 26490) + + namespace gsl +{ + // forward declare span<>: + + template + class span; + + // C++11 emulation: + + namespace std11 + { +#if gsl_HAVE(ADD_CONST) + + using std::add_const; + +#elif gsl_HAVE(TR1_ADD_CONST) + + using std::tr1::add_const; + +#else + + template + struct add_const + { + typedef const T type; + }; + +#endif // gsl_HAVE( ADD_CONST ) + +#if gsl_HAVE(REMOVE_CONST) + + using std::remove_const; + using std::remove_cv; + using std::remove_volatile; + +#elif gsl_HAVE(TR1_REMOVE_CONST) + + using std::tr1::remove_const; + using std::tr1::remove_cv; + using std::tr1::remove_volatile; + +#else + + template + struct remove_const + { + typedef T type; + }; + template + struct remove_const + { + typedef T type; + }; + + template + struct remove_volatile + { + typedef T type; + }; + template + struct remove_volatile + { + typedef T type; + }; + + template + struct remove_cv + { + typedef typename remove_volatile::type>::type type; + }; + +#endif // gsl_HAVE( REMOVE_CONST ) + +#if gsl_HAVE(INTEGRAL_CONSTANT) + + using std::false_type; + using std::integral_constant; + using std::true_type; + +#elif gsl_HAVE(TR1_INTEGRAL_CONSTANT) + + using std::tr1::false_type; + using std::tr1::integral_constant; + using std::tr1::true_type; + +#else + + template + struct integral_constant + { + enum + { + value = v + }; + }; + typedef integral_constant true_type; + typedef integral_constant false_type; + +#endif + + } // namespace std11 + + // C++17 emulation: + + namespace std17 + { + template + struct bool_constant : std11::integral_constant + { + }; + +#if gsl_CPP11_120 + + template + using void_t = void; + +#endif + +#if gsl_HAVE(STD_DATA) + + using std::data; + using std::size; + +#elif gsl_HAVE(CONSTRAINED_SPAN_CONTAINER_CTOR) + + template + inline gsl_constexpr auto size(const T (&)[N]) gsl_noexcept -> size_t + { + return N; + } + + template + inline gsl_constexpr auto size(C const &cont) -> decltype(cont.size()) + { + return cont.size(); + } + + template + inline gsl_constexpr auto data(T (&arr)[N]) gsl_noexcept -> T * + { + return &arr[0]; + } + + template + inline gsl_constexpr auto data(C &cont) -> decltype(cont.data()) + { + return cont.data(); + } + + template + inline gsl_constexpr auto data(C const &cont) -> decltype(cont.data()) + { + return cont.data(); + } + + template + inline gsl_constexpr auto data(std::initializer_list il) gsl_noexcept -> E const * + { + return il.begin(); + } + +#endif // span_HAVE( DATA ) + + } // namespace std17 + + namespace detail + { + /// for nsel_REQUIRES_T + + /*enum*/ class enabler + { + }; + +#if gsl_HAVE(TYPE_TRAITS) + + template + struct is_span_oracle : std::false_type + { + }; + + template + struct is_span_oracle > : std::true_type + { + }; + + template + struct is_span : is_span_oracle::type> + { + }; + + template + struct is_std_array_oracle : std::false_type + { + }; + +#if gsl_HAVE(ARRAY) + + template + struct is_std_array_oracle > : std::true_type + { + }; + +#endif + + template + struct is_std_array : is_std_array_oracle::type> + { + }; + + template + struct is_array : std::false_type + { + }; + + template + struct is_array : std::true_type + { + }; + + template + struct is_array : std::true_type + { + }; + +#if gsl_CPP11_140 && !gsl_BETWEEN(gsl_COMPILER_GNUC_VERSION, 1, 500) + + template + struct has_size_and_data : std::false_type + { + }; + + template + struct has_size_and_data< + C, std17::void_t< + decltype(std17::size(std::declval())), + decltype(std17::data(std::declval()))> > : std::true_type + { + }; + + template + struct is_compatible_element : std::false_type + { + }; + + template + struct is_compatible_element< + C, E, std17::void_t()))> > : std::is_convertible()))>::type (*)[], E (*)[]> + { + }; + + template + struct is_container : std17::bool_constant< + !is_span::value && !is_array::value && !is_std_array::value && has_size_and_data::value> + { + }; + + template + struct is_compatible_container : std17::bool_constant< + is_container::value && is_compatible_element::value> + { + }; + +#else // gsl_CPP11_140 + + template < + class C, class E gsl_REQUIRES_T((!is_span::value && !is_array::value && !is_std_array::value && (std::is_convertible()))>::type (*)[], E (*)[]>::value) + // && has_size_and_data< C >::value + )), + class = decltype(std17::size(std::declval())), class = decltype(std17::data(std::declval()))> + struct is_compatible_container : std::true_type + { + }; + +#endif // gsl_CPP11_140 + +#endif // gsl_HAVE( TYPE_TRAITS ) + + } // namespace detail + + // + // GSL.util: utilities + // + + // index type for all container indexes/subscripts/sizes + typedef gsl_CONFIG_SPAN_INDEX_TYPE index; // p0122r3 uses std::ptrdiff_t + +// +// GSL.owner: ownership pointers +// +#if gsl_HAVE(SHARED_PTR) + using std::make_shared; + using std::shared_ptr; + using std::unique_ptr; +#if gsl_HAVE(MAKE_UNIQUE) + using std::make_unique; +#endif +#endif + +#if gsl_HAVE(ALIAS_TEMPLATE) +#if gsl_HAVE(TYPE_TRAITS) + template ::value)> + using owner = T; +#else + template + using owner = T; +#endif +#else + template + struct owner + { + typedef T type; + }; +#endif + +#define gsl_HAVE_OWNER_TEMPLATE gsl_HAVE_ALIAS_TEMPLATE + +#if gsl_FEATURE(OWNER_MACRO) +#if gsl_HAVE(OWNER_TEMPLATE) +#define Owner(t) ::gsl::owner +#else +#define Owner(t) ::gsl::owner::type +#endif +#endif + + // + // GSL.assert: assertions + // + +#define gsl_ELIDE_CONTRACT_EXPECTS (0 == (gsl_CONFIG_CONTRACT_LEVEL_MASK & 0x01)) +#define gsl_ELIDE_CONTRACT_ENSURES (0 == (gsl_CONFIG_CONTRACT_LEVEL_MASK & 0x10)) + +#if gsl_ELIDE_CONTRACT_EXPECTS +#define Expects(x) /* Expects elided */ +#elif gsl_CONFIG(CONTRACT_VIOLATION_THROWS_V) +#define Expects(x) ::gsl::fail_fast_assert((x), "GSL: Precondition failure at " __FILE__ ":" gsl_STRINGIFY(__LINE__)) +#elif gsl_CONFIG(CONTRACT_VIOLATION_CALLS_HANDLER_V) +#define Expects(x) ::gsl::fail_fast_assert((x), #x, "GSL: Precondition failure", __FILE__, __LINE__) +#else +#define Expects(x) ::gsl::fail_fast_assert((x)) +#endif + +#if gsl_ELIDE_CONTRACT_EXPECTS +#define gsl_EXPECTS_UNUSED_PARAM(x) /* Make param unnamed if Expects elided */ +#else +#define gsl_EXPECTS_UNUSED_PARAM(x) x +#endif + +#if gsl_ELIDE_CONTRACT_ENSURES +#define Ensures(x) /* Ensures elided */ +#elif gsl_CONFIG(CONTRACT_VIOLATION_THROWS_V) +#define Ensures(x) ::gsl::fail_fast_assert((x), "GSL: Postcondition failure at " __FILE__ ":" gsl_STRINGIFY(__LINE__)) +#elif gsl_CONFIG(CONTRACT_VIOLATION_CALLS_HANDLER_V) +#define Ensures(x) ::gsl::fail_fast_assert((x), #x, "GSL: Postcondition failure", __FILE__, __LINE__) +#else +#define Ensures(x) ::gsl::fail_fast_assert((x)) +#endif + +#define gsl_STRINGIFY(x) gsl_STRINGIFY_(x) +#define gsl_STRINGIFY_(x) #x + + struct fail_fast : public std::logic_error + { + gsl_api explicit fail_fast(char const *const message) + : std::logic_error(message) {} + }; + +#if gsl_CONFIG(CONTRACT_VIOLATION_THROWS_V) + + gsl_api inline void fail_fast_assert(bool cond, char const *const message) + { + if (!cond) + throw fail_fast(message); + } + +#elif gsl_CONFIG(CONTRACT_VIOLATION_CALLS_HANDLER_V) + + // Should be defined by user + gsl_api void fail_fast_assert_handler(char const *const expression, char const *const message, char const *const file, int line); + + gsl_api inline void fail_fast_assert(bool cond, char const *const expression, char const *const message, char const *const file, int line) + { + if (!cond) + fail_fast_assert_handler(expression, message, file, line); + } + +#else + + gsl_api inline void fail_fast_assert(bool cond) gsl_noexcept + { +#ifdef __CUDA_ARCH__ + assert(cond); +#else // __CUDA_ARCH__ + if (!cond) + std::terminate(); +#endif // __CUDA_ARCH__ + } + +#endif + + // + // GSL.util: utilities + // + +#if gsl_FEATURE(EXPERIMENTAL_RETURN_GUARD) + + // Add uncaught_exceptions for pre-2017 MSVC, GCC and Clang + // Return unsigned char to save stack space, uncaught_exceptions can only increase by 1 in a scope + + namespace detail + { + inline unsigned char to_uchar(unsigned x) gsl_noexcept + { + return static_cast(x); + } + + } // namespace detail + + namespace std11 + { +#if gsl_HAVE(UNCAUGHT_EXCEPTIONS) + + inline unsigned char uncaught_exceptions() gsl_noexcept + { + return detail::to_uchar(std::uncaught_exceptions()); + } + +#elif gsl_COMPILER_MSVC_VERSION + + extern "C" char *__cdecl _getptd(); + inline unsigned char uncaught_exceptions() gsl_noexcept + { + return detail::to_uchar(*reinterpret_cast(_getptd() + (sizeof(void *) == 8 ? 0x100 : 0x90))); + } + +#elif gsl_COMPILER_CLANG_VERSION || gsl_COMPILER_GNUC_VERSION + + extern "C" char *__cxa_get_globals(); + inline unsigned char uncaught_exceptions() gsl_noexcept + { + return detail::to_uchar(*reinterpret_cast(__cxa_get_globals() + sizeof(void *))); + } +#endif + } // namespace std11 +#endif + +#if gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION >= 110 + + template + class final_action + { + public: + gsl_api explicit final_action(F action) gsl_noexcept + : action_(std::move(action)), + invoke_(true) + { + } + + gsl_api final_action(final_action &&other) gsl_noexcept + : action_(std::move(other.action_)), + invoke_(other.invoke_) + { + other.invoke_ = false; + } + + gsl_api virtual ~final_action() gsl_noexcept + { + if (invoke_) + action_(); + } + + gsl_is_delete_access : gsl_api final_action(final_action const &) gsl_is_delete; + gsl_api final_action &operator=(final_action const &) gsl_is_delete; + gsl_api final_action &operator=(final_action &&) gsl_is_delete; + + protected: + gsl_api void dismiss() gsl_noexcept + { + invoke_ = false; + } + + private: + F action_; + bool invoke_; + }; + + template + gsl_api inline final_action finally(F const &action) gsl_noexcept + { + return final_action(action); + } + + template + gsl_api inline final_action finally(F && action) gsl_noexcept + { + return final_action(std::forward(action)); + } + +#if gsl_FEATURE(EXPERIMENTAL_RETURN_GUARD) + + template + class final_action_return : public final_action + { + public: + gsl_api explicit final_action_return(F &&action) gsl_noexcept + : final_action(std::move(action)), + exception_count(std11::uncaught_exceptions()) + { + } + + gsl_api final_action_return(final_action_return &&other) gsl_noexcept + : final_action(std::move(other)), + exception_count(std11::uncaught_exceptions()) + { + } + + gsl_api ~final_action_return() override + { + if (std11::uncaught_exceptions() != exception_count) + this->dismiss(); + } + + gsl_is_delete_access : gsl_api final_action_return(final_action_return const &) gsl_is_delete; + gsl_api final_action_return &operator=(final_action_return const &) gsl_is_delete; + + private: + unsigned char exception_count; + }; + + template + gsl_api inline final_action_return on_return(F const &action) gsl_noexcept + { + return final_action_return(action); + } + + template + gsl_api inline final_action_return on_return(F && action) gsl_noexcept + { + return final_action_return(std::forward(action)); + } + + template + class final_action_error : public final_action + { + public: + gsl_api explicit final_action_error(F &&action) gsl_noexcept + : final_action(std::move(action)), + exception_count(std11::uncaught_exceptions()) + { + } + + gsl_api final_action_error(final_action_error &&other) gsl_noexcept + : final_action(std::move(other)), + exception_count(std11::uncaught_exceptions()) + { + } + + gsl_api ~final_action_error() override + { + if (std11::uncaught_exceptions() == exception_count) + this->dismiss(); + } + + gsl_is_delete_access : gsl_api final_action_error(final_action_error const &) gsl_is_delete; + gsl_api final_action_error &operator=(final_action_error const &) gsl_is_delete; + + private: + unsigned char exception_count; + }; + + template + gsl_api inline final_action_error on_error(F const &action) gsl_noexcept + { + return final_action_error(action); + } + + template + gsl_api inline final_action_error on_error(F && action) gsl_noexcept + { + return final_action_error(std::forward(action)); + } + +#endif // gsl_FEATURE( EXPERIMENTAL_RETURN_GUARD ) + +#else // gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION >= 110 + + class final_action + { + public: + typedef void (*Action)(); + + gsl_api final_action(Action action) + : action_(action), invoke_(true) + { + } + + gsl_api final_action(final_action const &other) + : action_(other.action_), invoke_(other.invoke_) + { + other.invoke_ = false; + } + + gsl_api virtual ~final_action() + { + if (invoke_) + action_(); + } + + protected: + gsl_api void dismiss() + { + invoke_ = false; + } + + private: + gsl_api final_action &operator=(final_action const &); + + private: + Action action_; + mutable bool invoke_; + }; + + template + gsl_api inline final_action finally(F const &f) + { + return final_action((f)); + } + +#if gsl_FEATURE(EXPERIMENTAL_RETURN_GUARD) + + class final_action_return : public final_action + { + public: + gsl_api explicit final_action_return(Action action) + : final_action(action), exception_count(std11::uncaught_exceptions()) + { + } + + gsl_api ~final_action_return() + { + if (std11::uncaught_exceptions() != exception_count) + this->dismiss(); + } + + private: + gsl_api final_action_return &operator=(final_action_return const &); + + private: + unsigned char exception_count; + }; + + template + gsl_api inline final_action_return on_return(F const &action) + { + return final_action_return(action); + } + + class final_action_error : public final_action + { + public: + gsl_api explicit final_action_error(Action action) + : final_action(action), exception_count(std11::uncaught_exceptions()) + { + } + + gsl_api ~final_action_error() + { + if (std11::uncaught_exceptions() == exception_count) + this->dismiss(); + } + + private: + gsl_api final_action_error &operator=(final_action_error const &); + + private: + unsigned char exception_count; + }; + + template + gsl_api inline final_action_error on_error(F const &action) + { + return final_action_error(action); + } + +#endif // gsl_FEATURE( EXPERIMENTAL_RETURN_GUARD ) + +#endif // gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION == 110 + +#if gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION >= 120 + + template + gsl_api inline gsl_constexpr T narrow_cast(U && u) gsl_noexcept + { + return static_cast(std::forward(u)); + } + +#else + + template + gsl_api inline T narrow_cast(U u) gsl_noexcept + { + return static_cast(u); + } + +#endif // gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION >= 120 + + struct narrowing_error : public std::exception + { + }; + +#if gsl_HAVE(TYPE_TRAITS) + + namespace detail + { + template + struct is_same_signedness : public std::integral_constant::value == std::is_signed::value> + { + }; + } // namespace detail +#endif + + template + gsl_api inline T narrow(U u) + { + T t = narrow_cast(u); + + if (static_cast(t) != u) + { +#if gsl_CONFIG(CONTRACT_VIOLATION_THROWS_V) + throw narrowing_error(); +#else + std::terminate(); +#endif + } + +#if gsl_HAVE(TYPE_TRAITS) +#if gsl_COMPILER_MSVC_VERSION + // Suppress MSVC level 4 warning C4127 (conditional expression is constant) + if (0, !detail::is_same_signedness::value && ((t < T()) != (u < U()))) +#else + if (!detail::is_same_signedness::value && ((t < T()) != (u < U()))) +#endif +#else + // Don't assume T() works: + if ((t < 0) != (u < 0)) +#endif + { +#if gsl_CONFIG(CONTRACT_VIOLATION_THROWS_V) + throw narrowing_error(); +#else + std::terminate(); +#endif + } + return t; + } + + // + // at() - Bounds-checked way of accessing static arrays, std::array, std::vector. + // + + template + gsl_api inline gsl_constexpr14 T &at(T(&arr)[N], size_t pos) + { + Expects(pos < N); + return arr[pos]; + } + + template + gsl_api inline gsl_constexpr14 typename Container::value_type &at(Container & cont, size_t pos) + { + Expects(pos < cont.size()); + return cont[pos]; + } + + template + gsl_api inline gsl_constexpr14 typename Container::value_type const &at(Container const &cont, size_t pos) + { + Expects(pos < cont.size()); + return cont[pos]; + } + +#if gsl_HAVE(INITIALIZER_LIST) + + template + gsl_api inline const gsl_constexpr14 T at(std::initializer_list cont, size_t pos) + { + Expects(pos < cont.size()); + return *(cont.begin() + pos); + } +#endif + + template + gsl_api inline gsl_constexpr T &at(span s, size_t pos) + { + return s.at(pos); + } + + // + // GSL.views: views + // + + // + // not_null<> - Wrap any indirection and enforce non-null. + // + template + class not_null + { +#if gsl_CONFIG(NOT_NULL_EXPLICIT_CTOR) +#define gsl_not_null_explicit explicit +#else +#define gsl_not_null_explicit /*explicit*/ +#endif + +#if gsl_CONFIG(NOT_NULL_GET_BY_CONST_REF) + typedef T const &get_result_t; +#else + typedef T get_result_t; +#endif + + public: +#if gsl_HAVE(TYPE_TRAITS) + static_assert(std::is_assignable::value, "T cannot be assigned nullptr."); +#endif + + template ::value)) +#endif + > + gsl_api gsl_constexpr14 gsl_not_null_explicit +#if gsl_HAVE(RVALUE_REFERENCE) + not_null(U &&u) + : ptr_(std::forward(u)) +#else + not_null(U const &u) + : ptr_(u) +#endif + { + Expects(ptr_ != gsl_nullptr); + } +#undef gsl_not_null_explicit + +#if gsl_HAVE(IS_DEFAULT) + gsl_api ~not_null() = default; + gsl_api gsl_constexpr not_null(not_null &&other) = default; + gsl_api gsl_constexpr not_null(not_null const &other) = default; + gsl_api not_null &operator=(not_null &&other) = default; + gsl_api not_null &operator=(not_null const &other) = default; +#else + gsl_api ~not_null(){}; + gsl_api gsl_constexpr not_null(not_null const &other) : ptr_(other.ptr_) {} + gsl_api not_null &operator=(not_null const &other) + { + ptr_ = other.ptr_; + return *this; + } +#if gsl_HAVE(RVALUE_REFERENCE) + gsl_api gsl_constexpr not_null(not_null &&other) : ptr_(std::move(other.get())) + { + } + gsl_api not_null &operator=(not_null &&other) + { + ptr_ = std::move(other.get()); + return *this; + } +#endif +#endif + + template ::value)) +#endif + > + gsl_api gsl_constexpr not_null(not_null const &other) + : ptr_(other.get()) + { + } + + gsl_api gsl_constexpr14 get_result_t get() const + { + // Without cheating and changing ptr_ from the outside, this check is superfluous: + Ensures(ptr_ != gsl_nullptr); + return ptr_; + } + + gsl_api gsl_constexpr operator get_result_t() const { return get(); } + gsl_api gsl_constexpr get_result_t operator->() const { return get(); } + +#if gsl_HAVE(DECLTYPE_AUTO) + gsl_api gsl_constexpr decltype(auto) operator*() const + { + return *get(); + } +#endif + + gsl_is_delete_access : + // prevent compilation when initialized with a nullptr or literal 0: +#if gsl_HAVE(NULLPTR) + gsl_api not_null(std::nullptr_t) gsl_is_delete; + gsl_api not_null &operator=(std::nullptr_t) gsl_is_delete; +#else + gsl_api + not_null(int) gsl_is_delete; + gsl_api not_null &operator=(int) gsl_is_delete; +#endif + + // unwanted operators...pointers only point to single objects! + gsl_api not_null &operator++() gsl_is_delete; + gsl_api not_null &operator--() gsl_is_delete; + gsl_api not_null operator++(int) gsl_is_delete; + gsl_api not_null operator--(int) gsl_is_delete; + gsl_api not_null &operator+(size_t) gsl_is_delete; + gsl_api not_null &operator+=(size_t) gsl_is_delete; + gsl_api not_null &operator-(size_t) gsl_is_delete; + gsl_api not_null &operator-=(size_t) gsl_is_delete; + gsl_api not_null &operator+=(std::ptrdiff_t) gsl_is_delete; + gsl_api not_null &operator-=(std::ptrdiff_t) gsl_is_delete; + gsl_api void operator[](std::ptrdiff_t) const gsl_is_delete; + + private: + T ptr_; + }; + + // not_null with implicit constructor, allowing copy-initialization: + + template + class not_null_ic : public not_null + { + public: + template ::value)) +#endif + > + gsl_api gsl_constexpr14 +#if gsl_HAVE(RVALUE_REFERENCE) + not_null_ic(U &&u) + : not_null(std::forward(u)) +#else + not_null_ic(U const &u) + : not_null(u) +#endif + { + } + }; + + // more not_null unwanted operators + + template + std::ptrdiff_t operator-(not_null const &, not_null const &) gsl_is_delete; + + template + not_null operator-(not_null const &, std::ptrdiff_t) gsl_is_delete; + + template + not_null operator+(not_null const &, std::ptrdiff_t) gsl_is_delete; + + template + not_null operator+(std::ptrdiff_t, not_null const &) gsl_is_delete; + + // not_null comparisons + + template + gsl_api inline gsl_constexpr bool operator==(not_null const &l, not_null const &r) + { + return l.get() == r.get(); + } + + template + gsl_api inline gsl_constexpr bool operator<(not_null const &l, not_null const &r) + { + return l.get() < r.get(); + } + + template + gsl_api inline gsl_constexpr bool operator!=(not_null const &l, not_null const &r) + { + return !(l == r); + } + + template + gsl_api inline gsl_constexpr bool operator<=(not_null const &l, not_null const &r) + { + return !(r < l); + } + + template + gsl_api inline gsl_constexpr bool operator>(not_null const &l, not_null const &r) + { + return (r < l); + } + + template + gsl_api inline gsl_constexpr bool operator>=(not_null const &l, not_null const &r) + { + return !(l < r); + } + +// +// Byte-specific type. +// +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + enum class gsl_may_alias byte : unsigned char + { + }; +#else + struct gsl_may_alias byte + { + typedef unsigned char type; + type v; + }; +#endif + +#if gsl_HAVE(DEFAULT_FUNCTION_TEMPLATE_ARG) +#define gsl_ENABLE_IF_INTEGRAL_T(T) \ + gsl_REQUIRES_T((std::is_integral::value)) +#else +#define gsl_ENABLE_IF_INTEGRAL_T(T) +#endif + + template + gsl_api inline gsl_constexpr byte to_byte(T v) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return static_cast(v); +#elif gsl_HAVE(CONSTEXPR_11) + return {static_cast(v)}; +#else + byte b = {static_cast(v)}; + return b; +#endif + } + + template + gsl_api inline gsl_constexpr IntegerType to_integer(byte b) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return static_cast::type>(b); +#else + return b.v; +#endif + } + + gsl_api inline gsl_constexpr unsigned char to_uchar(byte b) gsl_noexcept + { + return to_integer(b); + } + + gsl_api inline gsl_constexpr unsigned char to_uchar(int i) gsl_noexcept + { + return static_cast(i); + } + +#if !gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + + gsl_api inline gsl_constexpr bool operator==(byte l, byte r) gsl_noexcept + { + return l.v == r.v; + } + + gsl_api inline gsl_constexpr bool operator!=(byte l, byte r) gsl_noexcept + { + return !(l == r); + } + + gsl_api inline gsl_constexpr bool operator<(byte l, byte r) gsl_noexcept + { + return l.v < r.v; + } + + gsl_api inline gsl_constexpr bool operator<=(byte l, byte r) gsl_noexcept + { + return !(r < l); + } + + gsl_api inline gsl_constexpr bool operator>(byte l, byte r) gsl_noexcept + { + return (r < l); + } + + gsl_api inline gsl_constexpr bool operator>=(byte l, byte r) gsl_noexcept + { + return !(l < r); + } +#endif + + template + gsl_api inline gsl_constexpr14 byte &operator<<=(byte &b, IntegerType shift) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return b = to_byte(to_uchar(b) << shift); +#else + b.v = to_uchar(b.v << shift); + return b; +#endif + } + + template + gsl_api inline gsl_constexpr byte operator<<(byte b, IntegerType shift) gsl_noexcept + { + return to_byte(to_uchar(b) << shift); + } + + template + gsl_api inline gsl_constexpr14 byte &operator>>=(byte &b, IntegerType shift) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return b = to_byte(to_uchar(b) >> shift); +#else + b.v = to_uchar(b.v >> shift); + return b; +#endif + } + + template + gsl_api inline gsl_constexpr byte operator>>(byte b, IntegerType shift) gsl_noexcept + { + return to_byte(to_uchar(b) >> shift); + } + + gsl_api inline gsl_constexpr14 byte &operator|=(byte &l, byte r) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return l = to_byte(to_uchar(l) | to_uchar(r)); +#else + l.v = to_uchar(l) | to_uchar(r); + return l; +#endif + } + + gsl_api inline gsl_constexpr byte operator|(byte l, byte r) gsl_noexcept + { + return to_byte(to_uchar(l) | to_uchar(r)); + } + + gsl_api inline gsl_constexpr14 byte &operator&=(byte &l, byte r) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return l = to_byte(to_uchar(l) & to_uchar(r)); +#else + l.v = to_uchar(l) & to_uchar(r); + return l; +#endif + } + + gsl_api inline gsl_constexpr byte operator&(byte l, byte r)gsl_noexcept + { + return to_byte(to_uchar(l) & to_uchar(r)); + } + + gsl_api inline gsl_constexpr14 byte &operator^=(byte &l, byte r) gsl_noexcept + { +#if gsl_HAVE(ENUM_CLASS_CONSTRUCTION_FROM_UNDERLYING_TYPE) + return l = to_byte(to_uchar(l) ^ to_uchar(r)); +#else + l.v = to_uchar(l) ^ to_uchar(r); + return l; +#endif + } + + gsl_api inline gsl_constexpr byte operator^(byte l, byte r) gsl_noexcept + { + return to_byte(to_uchar(l) ^ to_uchar(r)); + } + + gsl_api inline gsl_constexpr byte operator~(byte b) gsl_noexcept + { + return to_byte(~to_uchar(b)); + } + +#if gsl_FEATURE_TO_STD(WITH_CONTAINER) + + // Tag to select span constructor taking a container (prevent ms-gsl warning C26426): + + struct with_container_t + { + gsl_constexpr with_container_t() gsl_noexcept {} + }; + const gsl_constexpr with_container_t with_container; + +#endif + + // + // span<> - A 1D view of contiguous T's, replace (*,len). + // + template + class span + { + template + friend class span; + + public: + typedef index index_type; + + typedef T element_type; + typedef typename std11::remove_cv::type value_type; + + typedef T &reference; + typedef T *pointer; + typedef T const *const_pointer; + typedef T const &const_reference; + + typedef pointer iterator; + typedef const_pointer const_iterator; + + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; + + typedef typename std::iterator_traits::difference_type difference_type; + + // 26.7.3.2 Constructors, copy, and assignment [span.cons] + + gsl_api gsl_constexpr14 span() gsl_noexcept + : first_(gsl_nullptr), + last_(gsl_nullptr) + { + Expects(size() == 0); + } + +#if !gsl_DEPRECATE_TO_LEVEL(5) + +#if gsl_HAVE(NULLPTR) + gsl_api gsl_constexpr14 span(std::nullptr_t, index_type gsl_EXPECTS_UNUSED_PARAM(size_in)) + : first_(nullptr), last_(nullptr) + { + Expects(size_in == 0); + } +#endif + +#if gsl_HAVE(IS_DELETE) + gsl_api gsl_constexpr span(reference data_in) + : span(&data_in, 1) + { + } + + gsl_api gsl_constexpr span(element_type &&) = delete; +#endif + +#endif // deprecate + + gsl_api gsl_constexpr14 span(pointer data_in, index_type size_in) + : first_(data_in), last_(data_in + size_in) + { + Expects(size_in == 0 || (size_in > 0 && data_in != gsl_nullptr)); + } + + gsl_api gsl_constexpr14 span(pointer first_in, pointer last_in) + : first_(first_in), last_(last_in) + { + Expects(first_in <= last_in); + } + +#if !gsl_DEPRECATE_TO_LEVEL(5) + + template + gsl_api gsl_constexpr14 span(U *&data_in, index_type size_in) + : first_(data_in), last_(data_in + size_in) + { + Expects(size_in == 0 || (size_in > 0 && data_in != gsl_nullptr)); + } + + template + gsl_api gsl_constexpr14 span(U *const &data_in, index_type size_in) + : first_(data_in), last_(data_in + size_in) + { + Expects(size_in == 0 || (size_in > 0 && data_in != gsl_nullptr)); + } + +#endif // deprecate + +#if !gsl_DEPRECATE_TO_LEVEL(5) + template + gsl_api gsl_constexpr span(U (&arr)[N]) gsl_noexcept + : first_(gsl_ADDRESSOF(arr[0])), + last_(gsl_ADDRESSOF(arr[0]) + N) + { + } +#else + template ::value)) +#endif + > + gsl_api gsl_constexpr span(element_type (&arr)[N]) gsl_noexcept + : first_(gsl_ADDRESSOF(arr[0])), + last_(gsl_ADDRESSOF(arr[0]) + N) + { + } +#endif // deprecate + +#if gsl_HAVE(ARRAY) +#if !gsl_DEPRECATE_TO_LEVEL(5) + + template + gsl_api gsl_constexpr span(std::array &arr) + : first_(arr.data()), last_(arr.data() + N) + { + } + + template + gsl_api gsl_constexpr span(std::array const &arr) + : first_(arr.data()), last_(arr.data() + N) + { + } + +#else + + template ::value)) +#endif + > + gsl_api gsl_constexpr span(std::array &arr) + : first_(arr.data()), last_(arr.data() + N) + { + } + + template ::value)) +#endif + > + gsl_api gsl_constexpr span(std::array const &arr) + : first_(arr.data()), last_(arr.data() + N) + { + } + +#endif // deprecate +#endif // gsl_HAVE( ARRAY ) + +#if gsl_HAVE(CONSTRAINED_SPAN_CONTAINER_CTOR) + template ::value))> + gsl_api gsl_constexpr span(Container &cont) + : first_(std17::data(cont)), last_(std17::data(cont) + std17::size(cont)) + { + } + + template ::value && detail::is_compatible_container::value))> + gsl_api gsl_constexpr span(Container const &cont) + : first_(std17::data(cont)), last_(std17::data(cont) + std17::size(cont)) + { + } + +#elif gsl_HAVE(UNCONSTRAINED_SPAN_CONTAINER_CTOR) + + template + gsl_api gsl_constexpr span(Container &cont) + : first_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0])), last_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0]) + cont.size()) + { + } + + template + gsl_api gsl_constexpr span(Container const &cont) + : first_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0])), last_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0]) + cont.size()) + { + } + +#endif + +#if gsl_FEATURE_TO_STD(WITH_CONTAINER) + + template + gsl_api gsl_constexpr span(with_container_t, Container &cont) + : first_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0])), last_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0]) + cont.size()) + { + } + + template + gsl_api gsl_constexpr span(with_container_t, Container const &cont) + : first_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0])), last_(cont.size() == 0 ? gsl_nullptr : gsl_ADDRESSOF(cont[0]) + cont.size()) + { + } + +#endif + +#if !gsl_DEPRECATE_TO_LEVEL(4) + // constructor taking shared_ptr deprecated since 0.29.0 + +#if gsl_HAVE(SHARED_PTR) + gsl_api gsl_constexpr span(shared_ptr const &ptr) + : first_(ptr.get()), last_(ptr.get() ? ptr.get() + 1 : gsl_nullptr) + { + } +#endif + + // constructors taking unique_ptr deprecated since 0.29.0 + +#if gsl_HAVE(UNIQUE_PTR) +#if gsl_HAVE(DEFAULT_FUNCTION_TEMPLATE_ARG) + template ::type> +#else + template +#endif + gsl_api gsl_constexpr span(unique_ptr const &ptr, index_type count) + : first_(ptr.get()), last_(ptr.get() + count) + { + } + + gsl_api gsl_constexpr span(unique_ptr const &ptr) + : first_(ptr.get()), last_(ptr.get() ? ptr.get() + 1 : gsl_nullptr) + { + } +#endif + +#endif // deprecate shared_ptr, unique_ptr + +#if gsl_HAVE(IS_DEFAULT) && !gsl_BETWEEN(gsl_COMPILER_GNUC_VERSION, 430, 600) + gsl_api gsl_constexpr span(span &&) gsl_noexcept = default; + gsl_api gsl_constexpr span(span const &) = default; +#else + gsl_api gsl_constexpr span(span const &other) + : first_(other.begin()), last_(other.end()) + { + } +#endif + +#if gsl_HAVE(IS_DEFAULT) + ~span() = default; +#else + ~span() + { + } +#endif + +#if gsl_HAVE(IS_DEFAULT) + gsl_api gsl_constexpr14 span &operator=(span &&) gsl_noexcept = default; + gsl_api gsl_constexpr14 span &operator=(span const &) gsl_noexcept = default; +#else + gsl_api span &operator=(span other) gsl_noexcept + { + other.swap(*this); + return *this; + } +#endif + + template ::value)) +#endif + > + gsl_api gsl_constexpr span(span const &other) + : first_(other.begin()), last_(other.end()) + { + } + +#if 0 + // Converting from other span ? + template< class U > operator=(); +#endif + + // 26.7.3.3 Subviews [span.sub] + + gsl_api gsl_constexpr14 span first(index_type count) const gsl_noexcept + { + Expects(0 <= count && count <= this->size()); + return span(this->data(), count); + } + + gsl_api gsl_constexpr14 span last(index_type count) const gsl_noexcept + { + Expects(0 <= count && count <= this->size()); + return span(this->data() + this->size() - count, count); + } + + gsl_api gsl_constexpr14 span subspan(index_type offset) const gsl_noexcept + { + Expects(0 <= offset && offset <= this->size()); + return span(this->data() + offset, this->size() - offset); + } + + gsl_api gsl_constexpr14 span subspan(index_type offset, index_type count) const gsl_noexcept + { + Expects( + 0 <= offset && offset <= this->size() && + 0 <= count && count <= this->size() - offset); + return span(this->data() + offset, count); + } + + // 26.7.3.4 Observers [span.obs] + + gsl_api gsl_constexpr index_type size() const gsl_noexcept + { + return narrow_cast(last_ - first_); + } + + gsl_api gsl_constexpr std::ptrdiff_t ssize() const gsl_noexcept + { + return narrow_cast(last_ - first_); + } + + gsl_api gsl_constexpr index_type size_bytes() const gsl_noexcept + { + return size() * narrow_cast(sizeof(element_type)); + } + + gsl_api gsl_constexpr bool empty() const gsl_noexcept + { + return size() == 0; + } + + // 26.7.3.5 Element access [span.elem] + + gsl_api gsl_constexpr reference operator[](index_type pos) const + { + return at(pos); + } + + gsl_api gsl_constexpr reference operator()(index_type pos) const + { + return at(pos); + } + + gsl_api gsl_constexpr14 reference at(index_type pos) const + { + Expects(pos < size()); + return first_[pos]; + } + + gsl_api gsl_constexpr pointer data() const gsl_noexcept + { + return first_; + } + + // 26.7.3.6 Iterator support [span.iterators] + + gsl_api gsl_constexpr iterator begin() const gsl_noexcept + { + return iterator(first_); + } + + gsl_api gsl_constexpr iterator end() const gsl_noexcept + { + return iterator(last_); + } + + gsl_api gsl_constexpr const_iterator cbegin() const gsl_noexcept + { +#if gsl_CPP11_OR_GREATER + return {begin()}; +#else + return const_iterator(begin()); +#endif + } + + gsl_api gsl_constexpr const_iterator cend() const gsl_noexcept + { +#if gsl_CPP11_OR_GREATER + return {end()}; +#else + return const_iterator(end()); +#endif + } + + gsl_api gsl_constexpr reverse_iterator rbegin() const gsl_noexcept + { + return reverse_iterator(end()); + } + + gsl_api gsl_constexpr reverse_iterator rend() const gsl_noexcept + { + return reverse_iterator(begin()); + } + + gsl_api gsl_constexpr const_reverse_iterator crbegin() const gsl_noexcept + { + return const_reverse_iterator(cend()); + } + + gsl_api gsl_constexpr const_reverse_iterator crend() const gsl_noexcept + { + return const_reverse_iterator(cbegin()); + } + + gsl_api void swap(span &other) gsl_noexcept + { + using std::swap; + swap(first_, other.first_); + swap(last_, other.last_); + } + +#if !gsl_DEPRECATE_TO_LEVEL(3) + // member length() deprecated since 0.29.0 + + gsl_api gsl_constexpr index_type length() const gsl_noexcept + { + return size(); + } + + // member length_bytes() deprecated since 0.29.0 + + gsl_api gsl_constexpr index_type length_bytes() const gsl_noexcept + { + return size_bytes(); + } +#endif + +#if !gsl_DEPRECATE_TO_LEVEL(2) + // member as_bytes(), as_writeable_bytes deprecated since 0.17.0 + + gsl_api span as_bytes() const gsl_noexcept + { + return span(reinterpret_cast(data()), size_bytes()); // NOLINT + } + + gsl_api span as_writeable_bytes() const gsl_noexcept + { + return span(reinterpret_cast(data()), size_bytes()); // NOLINT + } + +#endif + + template + gsl_api span as_span() const gsl_noexcept + { + Expects((this->size_bytes() % sizeof(U)) == 0); + return span(reinterpret_cast(this->data()), this->size_bytes() / sizeof(U)); // NOLINT + } + + private: + pointer first_; + pointer last_; + }; + + // class template argument deduction guides: + +#if gsl_HAVE(DEDUCTION_GUIDES) // gsl_CPP17_OR_GREATER + + template + span(T(&)[N])->span; + + template + span(std::array &)->span; + + template + span(std::array const &)->span; + + template + span(Container &)->span; + + template + span(Container const &)->span; + +#endif // gsl_HAVE( DEDUCTION_GUIDES ) + + // 26.7.3.7 Comparison operators [span.comparison] + +#if gsl_CONFIG(ALLOWS_NONSTRICT_SPAN_COMPARISON) + + template + gsl_api inline gsl_constexpr bool operator==(span const &l, span const &r) + { + return l.size() == r.size() && (l.begin() == r.begin() || std::equal(l.begin(), l.end(), r.begin())); + } + + template + gsl_api inline gsl_constexpr bool operator<(span const &l, span const &r) + { + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); + } + +#else + + template + gsl_api inline gsl_constexpr bool operator==(span const &l, span const &r) + { + return l.size() == r.size() && (l.begin() == r.begin() || std::equal(l.begin(), l.end(), r.begin())); + } + + template + gsl_api inline gsl_constexpr bool operator<(span const &l, span const &r) + { + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); + } +#endif + + template + gsl_api inline gsl_constexpr bool operator!=(span const &l, span const &r) + { + return !(l == r); + } + + template + gsl_api inline gsl_constexpr bool operator<=(span const &l, span const &r) + { + return !(r < l); + } + + template + gsl_api inline gsl_constexpr bool operator>(span const &l, span const &r) + { + return (r < l); + } + + template + gsl_api inline gsl_constexpr bool operator>=(span const &l, span const &r) + { + return !(l < r); + } + + // span algorithms + + template + gsl_api inline gsl_constexpr std::size_t size(span const &spn) + { + return static_cast(spn.size()); + } + + template + gsl_api inline gsl_constexpr std::ptrdiff_t ssize(span const &spn) + { + return spn.ssize(); + } + + namespace detail + { + template + gsl_api inline OI copy_n(II first, N count, OI result) + { + if (count > 0) + { + *result++ = *first; + for (N i = 1; i < count; ++i) + { + *result++ = *++first; + } + } + return result; + } + } // namespace detail + + template + gsl_api inline void copy(span src, span dest) + { +#if gsl_CPP14_OR_GREATER // gsl_HAVE( TYPE_TRAITS ) (circumvent Travis clang 3.4) + static_assert(std::is_assignable::value, "Cannot assign elements of source span to elements of destination span"); +#endif + Expects(dest.size() >= src.size()); + detail::copy_n(src.data(), src.size(), dest.data()); + } + + // span creator functions (see ctors) + + template + gsl_api inline span as_bytes(span spn) gsl_noexcept + { + return span(reinterpret_cast(spn.data()), spn.size_bytes()); // NOLINT + } + + template + gsl_api inline span as_writeable_bytes(span spn) gsl_noexcept + { + return span(reinterpret_cast(spn.data()), spn.size_bytes()); // NOLINT + } + +#if gsl_FEATURE_TO_STD(MAKE_SPAN) + + template + gsl_api inline gsl_constexpr span + make_span(T * ptr, typename span::index_type count) + { + return span(ptr, count); + } + + template + gsl_api inline gsl_constexpr span + make_span(T * first, T * last) + { + return span(first, last); + } + + template + gsl_api inline gsl_constexpr span + make_span(T(&arr)[N]) + { + return span(gsl_ADDRESSOF(arr[0]), N); + } + +#if gsl_HAVE(ARRAY) + + template + gsl_api inline gsl_constexpr span + make_span(std::array & arr) + { + return span(arr); + } + + template + gsl_api inline gsl_constexpr span + make_span(std::array const &arr) + { + return span(arr); + } +#endif + +#if gsl_HAVE(CONSTRAINED_SPAN_CONTAINER_CTOR) && gsl_HAVE(AUTO) + + template ()))> + gsl_api inline gsl_constexpr auto + make_span(Container & cont) + ->span::type> + { + return span::type>(cont); + } + + template ()))> + gsl_api inline gsl_constexpr auto + make_span(Container const &cont) + ->span::type> + { + return span::type>(cont); + } + +#else + + template + gsl_api inline span + make_span(std::vector & cont) + { + return span(with_container, cont); + } + + template + gsl_api inline span + make_span(std::vector const &cont) + { + return span(with_container, cont); + } +#endif + +#if gsl_FEATURE_TO_STD(WITH_CONTAINER) + + template + gsl_api inline gsl_constexpr span + make_span(with_container_t, Container & cont) gsl_noexcept + { + return span(with_container, cont); + } + + template + gsl_api inline gsl_constexpr span + make_span(with_container_t, Container const &cont) gsl_noexcept + { + return span(with_container, cont); + } + +#endif // gsl_FEATURE_TO_STD( WITH_CONTAINER ) + + template + gsl_api inline span + make_span(Ptr & ptr) + { + return span(ptr); + } + + template + gsl_api inline span + make_span(Ptr & ptr, typename span::index_type count) + { + return span(ptr, count); + } + +#endif // gsl_FEATURE_TO_STD( MAKE_SPAN ) + +#if gsl_FEATURE_TO_STD(BYTE_SPAN) + + template + gsl_api inline gsl_constexpr span + byte_span(T & t) gsl_noexcept + { + return span(reinterpret_cast(&t), sizeof(T)); + } + + template + gsl_api inline gsl_constexpr span + byte_span(T const &t) gsl_noexcept + { + return span(reinterpret_cast(&t), sizeof(T)); + } + +#endif // gsl_FEATURE_TO_STD( BYTE_SPAN ) + + // + // basic_string_span: + // + + template + class basic_string_span; + + namespace detail + { + template + struct is_basic_string_span_oracle : std11::false_type + { + }; + + template + struct is_basic_string_span_oracle > : std11::true_type + { + }; + + template + struct is_basic_string_span : is_basic_string_span_oracle::type> + { + }; + + template + gsl_api inline gsl_constexpr14 std::size_t string_length(T *ptr, std::size_t max) + { + if (ptr == gsl_nullptr || max <= 0) + return 0; + + std::size_t len = 0; + while (len < max && ptr[len]) // NOLINT + ++len; + + return len; + } + + } // namespace detail + + // + // basic_string_span<> - A view of contiguous characters, replace (*,len). + // + template + class basic_string_span + { + public: + typedef T element_type; + typedef span span_type; + + typedef typename span_type::index_type index_type; + typedef typename span_type::difference_type difference_type; + + typedef typename span_type::pointer pointer; + typedef typename span_type::reference reference; + + typedef typename span_type::iterator iterator; + typedef typename span_type::const_iterator const_iterator; + typedef typename span_type::reverse_iterator reverse_iterator; + typedef typename span_type::const_reverse_iterator const_reverse_iterator; + + // construction: + +#if gsl_HAVE(IS_DEFAULT) + gsl_api gsl_constexpr basic_string_span() gsl_noexcept = default; +#else + gsl_api gsl_constexpr basic_string_span() gsl_noexcept + { + } +#endif + +#if gsl_HAVE(NULLPTR) + gsl_api gsl_constexpr basic_string_span(std::nullptr_t ptr) gsl_noexcept + : span_(ptr, index_type(0)) + { + } +#endif + + gsl_api gsl_constexpr basic_string_span(pointer ptr) + : span_(remove_z(ptr, (std::numeric_limits::max)())) + { + } + + gsl_api gsl_constexpr basic_string_span(pointer ptr, index_type count) + : span_(ptr, count) + { + } + + gsl_api gsl_constexpr basic_string_span(pointer firstElem, pointer lastElem) + : span_(firstElem, lastElem) + { + } + + template + gsl_api gsl_constexpr basic_string_span(element_type (&arr)[N]) + : span_(remove_z(gsl_ADDRESSOF(arr[0]), N)) + { + } + +#if gsl_HAVE(ARRAY) + + template + gsl_api gsl_constexpr basic_string_span(std::array::type, N> &arr) + : span_(remove_z(arr)) + { + } + + template + gsl_api gsl_constexpr basic_string_span(std::array::type, N> const &arr) + : span_(remove_z(arr)) + { + } + +#endif + +#if gsl_HAVE(CONSTRAINED_SPAN_CONTAINER_CTOR) + + // Exclude: array, [basic_string,] basic_string_span + + template ::value && !detail::is_basic_string_span::value && std::is_convertible::value && std::is_convertible().data())>::value))> + gsl_api gsl_constexpr basic_string_span(Container &cont) + : span_((cont)) + { + } + + // Exclude: array, [basic_string,] basic_string_span + + template ::value && !detail::is_basic_string_span::value && std::is_convertible::value && std::is_convertible().data())>::value))> + gsl_api gsl_constexpr basic_string_span(Container const &cont) + : span_((cont)) + { + } + +#elif gsl_HAVE(UNCONSTRAINED_SPAN_CONTAINER_CTOR) + + template + gsl_api gsl_constexpr basic_string_span(Container &cont) + : span_(cont) + { + } + + template + gsl_api gsl_constexpr basic_string_span(Container const &cont) + : span_(cont) + { + } + +#else + + template + gsl_api gsl_constexpr basic_string_span(span const &rhs) + : span_(rhs) + { + } + +#endif + +#if gsl_FEATURE_TO_STD(WITH_CONTAINER) + + template + gsl_api gsl_constexpr basic_string_span(with_container_t, Container &cont) + : span_(with_container, cont) + { + } +#endif + +#if gsl_HAVE(IS_DEFAULT) +#if gsl_BETWEEN(gsl_COMPILER_GNUC_VERSION, 440, 600) + gsl_api gsl_constexpr basic_string_span(basic_string_span const &rhs) = default; + + gsl_api gsl_constexpr basic_string_span(basic_string_span &&rhs) = default; +#else + gsl_api gsl_constexpr basic_string_span(basic_string_span const &rhs) gsl_noexcept = default; + + gsl_api gsl_constexpr basic_string_span(basic_string_span &&rhs) gsl_noexcept = default; +#endif +#endif + + template ::pointer, pointer>::value)) +#endif + > + gsl_api gsl_constexpr basic_string_span(basic_string_span const &rhs) + : span_(reinterpret_cast(rhs.data()), rhs.length()) // NOLINT + { + } + +#if gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION >= 120 + template ::pointer, pointer>::value))> + gsl_api gsl_constexpr basic_string_span(basic_string_span &&rhs) + : span_(reinterpret_cast(rhs.data()), rhs.length()) // NOLINT + { + } +#endif + + template + gsl_api gsl_constexpr basic_string_span( + std::basic_string::type, CharTraits, Allocator> &str) + : span_(gsl_ADDRESSOF(str[0]), str.length()) + { + } + + template + gsl_api gsl_constexpr basic_string_span( + std::basic_string::type, CharTraits, Allocator> const &str) + : span_(gsl_ADDRESSOF(str[0]), str.length()) + { + } + + // destruction, assignment: + +#if gsl_HAVE(IS_DEFAULT) + gsl_api ~basic_string_span() gsl_noexcept = default; + + gsl_api basic_string_span &operator=(basic_string_span const &rhs) gsl_noexcept = default; + + gsl_api basic_string_span &operator=(basic_string_span &&rhs) gsl_noexcept = default; +#endif + + // sub span: + + gsl_api gsl_constexpr basic_string_span first(index_type count) const + { + return span_.first(count); + } + + gsl_api gsl_constexpr basic_string_span last(index_type count) const + { + return span_.last(count); + } + + gsl_api gsl_constexpr basic_string_span subspan(index_type offset) const + { + return span_.subspan(offset); + } + + gsl_api gsl_constexpr basic_string_span subspan(index_type offset, index_type count) const + { + return span_.subspan(offset, count); + } + + // observers: + + gsl_api gsl_constexpr index_type length() const gsl_noexcept + { + return span_.size(); + } + + gsl_api gsl_constexpr index_type size() const gsl_noexcept + { + return span_.size(); + } + + gsl_api gsl_constexpr index_type length_bytes() const gsl_noexcept + { + return span_.size_bytes(); + } + + gsl_api gsl_constexpr index_type size_bytes() const gsl_noexcept + { + return span_.size_bytes(); + } + + gsl_api gsl_constexpr bool empty() const gsl_noexcept + { + return size() == 0; + } + + gsl_api gsl_constexpr reference operator[](index_type idx) const + { + return span_[idx]; + } + + gsl_api gsl_constexpr reference operator()(index_type idx) const + { + return span_[idx]; + } + + gsl_api gsl_constexpr pointer data() const gsl_noexcept + { + return span_.data(); + } + + gsl_api iterator begin() const gsl_noexcept + { + return span_.begin(); + } + + gsl_api iterator end() const gsl_noexcept + { + return span_.end(); + } + + gsl_api reverse_iterator rbegin() const gsl_noexcept + { + return span_.rbegin(); + } + + gsl_api reverse_iterator rend() const gsl_noexcept + { + return span_.rend(); + } + + // const version not in p0123r2: + + gsl_api const_iterator cbegin() const gsl_noexcept + { + return span_.cbegin(); + } + + gsl_api const_iterator cend() const gsl_noexcept + { + return span_.cend(); + } + + gsl_api const_reverse_iterator crbegin() const gsl_noexcept + { + return span_.crbegin(); + } + + gsl_api const_reverse_iterator crend() const gsl_noexcept + { + return span_.crend(); + } + + private: + gsl_api static gsl_constexpr14 span_type remove_z(pointer const &sz, std::size_t max) + { + return span_type(sz, detail::string_length(sz, max)); + } + +#if gsl_HAVE(ARRAY) + template + gsl_api static gsl_constexpr14 span_type remove_z(std::array::type, N> &arr) + { + return remove_z(gsl_ADDRESSOF(arr[0]), narrow_cast(N)); + } + + template + gsl_api static gsl_constexpr14 span_type remove_z(std::array::type, N> const &arr) + { + return remove_z(gsl_ADDRESSOF(arr[0]), narrow_cast(N)); + } +#endif + + private: + span_type span_; + }; + + // basic_string_span comparison functions: + +#if gsl_CONFIG(ALLOWS_NONSTRICT_SPAN_COMPARISON) + + template + gsl_api inline gsl_constexpr14 bool operator==(basic_string_span const &l, U const &u) gsl_noexcept + { + const basic_string_span::type> r(u); + + return l.size() == r.size() && std::equal(l.begin(), l.end(), r.begin()); + } + + template + gsl_api inline gsl_constexpr14 bool operator<(basic_string_span const &l, U const &u) gsl_noexcept + { + const basic_string_span::type> r(u); + + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); + } + +#if gsl_HAVE(DEFAULT_FUNCTION_TEMPLATE_ARG) + + template ::value))> + gsl_api inline gsl_constexpr14 bool operator==(U const &u, basic_string_span const &r) gsl_noexcept + { + const basic_string_span::type> l(u); + + return l.size() == r.size() && std::equal(l.begin(), l.end(), r.begin()); + } + + template ::value))> + gsl_api inline gsl_constexpr14 bool operator<(U const &u, basic_string_span const &r) gsl_noexcept + { + const basic_string_span::type> l(u); + + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); + } +#endif + +#else //gsl_CONFIG( ALLOWS_NONSTRICT_SPAN_COMPARISON ) + + template + gsl_api inline gsl_constexpr14 bool operator==(basic_string_span const &l, basic_string_span const &r) gsl_noexcept + { + return l.size() == r.size() && std::equal(l.begin(), l.end(), r.begin()); + } + + template + gsl_api inline gsl_constexpr14 bool operator<(basic_string_span const &l, basic_string_span const &r) gsl_noexcept + { + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); + } + +#endif // gsl_CONFIG( ALLOWS_NONSTRICT_SPAN_COMPARISON ) + + template + gsl_api inline gsl_constexpr14 bool operator!=(basic_string_span const &l, U const &r) gsl_noexcept + { + return !(l == r); + } + + template + gsl_api inline gsl_constexpr14 bool operator<=(basic_string_span const &l, U const &r) gsl_noexcept + { +#if gsl_HAVE(DEFAULT_FUNCTION_TEMPLATE_ARG) || !gsl_CONFIG(ALLOWS_NONSTRICT_SPAN_COMPARISON) + return !(r < l); +#else + basic_string_span::type> rr(r); + return !(rr < l); +#endif + } + + template + gsl_api inline gsl_constexpr14 bool operator>(basic_string_span const &l, U const &r) gsl_noexcept + { +#if gsl_HAVE(DEFAULT_FUNCTION_TEMPLATE_ARG) || !gsl_CONFIG(ALLOWS_NONSTRICT_SPAN_COMPARISON) + return (r < l); +#else + basic_string_span::type> rr(r); + return (rr < l); +#endif + } + + template + gsl_api inline gsl_constexpr14 bool operator>=(basic_string_span const &l, U const &r) gsl_noexcept + { + return !(l < r); + } + +#if gsl_HAVE(DEFAULT_FUNCTION_TEMPLATE_ARG) + + template ::value))> + gsl_api inline gsl_constexpr14 bool operator!=(U const &l, basic_string_span const &r) gsl_noexcept + { + return !(l == r); + } + + template ::value))> + gsl_api inline gsl_constexpr14 bool operator<=(U const &l, basic_string_span const &r) gsl_noexcept + { + return !(r < l); + } + + template ::value))> + gsl_api inline gsl_constexpr14 bool operator>(U const &l, basic_string_span const &r) gsl_noexcept + { + return (r < l); + } + + template ::value))> + gsl_api inline gsl_constexpr14 bool operator>=(U const &l, basic_string_span const &r) gsl_noexcept + { + return !(l < r); + } + +#endif // gsl_HAVE( DEFAULT_FUNCTION_TEMPLATE_ARG ) + + // convert basic_string_span to byte span: + + template + gsl_api inline span as_bytes(basic_string_span spn) gsl_noexcept + { + return span(reinterpret_cast(spn.data()), spn.size_bytes()); // NOLINT + } + + // + // String types: + // + + typedef char *zstring; + typedef const char *czstring; + +#if gsl_HAVE(WCHAR) + typedef wchar_t *zwstring; + typedef const wchar_t *cwzstring; +#endif + + typedef basic_string_span string_span; + typedef basic_string_span cstring_span; + +#if gsl_HAVE(WCHAR) + typedef basic_string_span wstring_span; + typedef basic_string_span cwstring_span; +#endif + + // to_string() allow (explicit) conversions from string_span to string + +#if 0 + +template< class T > +gsl_api inline std::basic_string< typename std::remove_const::type > to_string( basic_string_span spn ) +{ + std::string( spn.data(), spn.length() ); +} + +#else + + gsl_api inline std::string to_string(string_span const &spn) + { + return std::string(spn.data(), spn.length()); + } + + gsl_api inline std::string to_string(cstring_span const &spn) + { + return std::string(spn.data(), spn.length()); + } + +#if gsl_HAVE(WCHAR) + + gsl_api inline std::wstring to_string(wstring_span const &spn) + { + return std::wstring(spn.data(), spn.length()); + } + + gsl_api inline std::wstring to_string(cwstring_span const &spn) + { + return std::wstring(spn.data(), spn.length()); + } + +#endif // gsl_HAVE( WCHAR ) +#endif // to_string() + + // + // Stream output for string_span types + // + + namespace detail + { + template + gsl_api void write_padding(Stream &os, std::streamsize n) + { + for (std::streamsize i = 0; i < n; ++i) + os.rdbuf()->sputc(os.fill()); + } + + template + gsl_api Stream &write_to_stream(Stream &os, Span const &spn) + { + typename Stream::sentry sentry(os); + + if (!os) + return os; + + const std::streamsize length = narrow(spn.length()); + + // Whether, and how, to pad + const bool pad = (length < os.width()); + const bool left_pad = pad && (os.flags() & std::ios_base::adjustfield) == std::ios_base::right; + + if (left_pad) + write_padding(os, os.width() - length); + + // Write span characters + os.rdbuf()->sputn(spn.begin(), length); + + if (pad && !left_pad) + write_padding(os, os.width() - length); + + // Reset output stream width + os.width(0); + + return os; + } + + } // namespace detail + + template + gsl_api std::basic_ostream &operator<<(std::basic_ostream &os, string_span const &spn) + { + return detail::write_to_stream(os, spn); + } + + template + gsl_api std::basic_ostream &operator<<(std::basic_ostream &os, cstring_span const &spn) + { + return detail::write_to_stream(os, spn); + } + +#if gsl_HAVE(WCHAR) + + template + gsl_api std::basic_ostream &operator<<(std::basic_ostream &os, wstring_span const &spn) + { + return detail::write_to_stream(os, spn); + } + + template + gsl_api std::basic_ostream &operator<<(std::basic_ostream &os, cwstring_span const &spn) + { + return detail::write_to_stream(os, spn); + } + +#endif // gsl_HAVE( WCHAR ) + + // + // ensure_sentinel() + // + // Provides a way to obtain a span from a contiguous sequence + // that ends with a (non-inclusive) sentinel value. + // + // Will fail-fast if sentinel cannot be found before max elements are examined. + // + namespace detail + { + template + gsl_api static span ensure_sentinel(T *seq, SizeType max = (std::numeric_limits::max)()) + { + typedef T *pointer; + + gsl_SUPPRESS_MSVC_WARNING(26429, "f.23: symbol 'cur' is never tested for nullness, it can be marked as not_null") + + pointer cur = seq; + + while (static_cast(cur - seq) < max && *cur != Sentinel) + ++cur; + + Expects(*cur == Sentinel); + + return span(seq, narrow_cast::index_type>(cur - seq)); + } + } // namespace detail + + // + // ensure_z - creates a string_span for a czstring or cwzstring. + // Will fail fast if a null-terminator cannot be found before + // the limit of size_type. + // + + template + gsl_api inline span ensure_z(T *const &sz, size_t max = (std::numeric_limits::max)()) + { + return detail::ensure_sentinel(sz, max); + } + + template + gsl_api inline span ensure_z(T(&sz)[N]) + { + return ensure_z(gsl_ADDRESSOF(sz[0]), N); + } + +#if gsl_HAVE(TYPE_TRAITS) + + template + gsl_api inline span::type> + ensure_z(Container & cont) + { + return ensure_z(cont.data(), cont.length()); + } +#endif + + // + // basic_zstring_span<> - A view of contiguous null-terminated characters, replace (*,len). + // + + template + class basic_zstring_span + { + public: + typedef T element_type; + typedef span span_type; + + typedef typename span_type::index_type index_type; + typedef typename span_type::difference_type difference_type; + + typedef element_type *czstring_type; + typedef basic_string_span string_span_type; + + gsl_api gsl_constexpr14 basic_zstring_span(span_type s) + : span_(s) + { + // expects a zero-terminated span + Expects(s[s.size() - 1] == '\0'); + } + +#if gsl_HAVE(IS_DEFAULT) + gsl_api gsl_constexpr basic_zstring_span(basic_zstring_span const &other) = default; + gsl_api gsl_constexpr basic_zstring_span(basic_zstring_span &&other) = default; + gsl_api gsl_constexpr14 basic_zstring_span &operator=(basic_zstring_span const &other) = default; + gsl_api gsl_constexpr14 basic_zstring_span &operator=(basic_zstring_span &&other) = default; +#else + gsl_api gsl_constexpr basic_zstring_span(basic_zstring_span const &other) : span_(other.span_) + { + } + gsl_api gsl_constexpr basic_zstring_span &operator=(basic_zstring_span const &other) + { + span_ = other.span_; + return *this; + } +#endif + + gsl_api gsl_constexpr bool empty() const gsl_noexcept + { + return span_.size() == 0; + } + + gsl_api gsl_constexpr string_span_type as_string_span() const gsl_noexcept + { + return string_span_type(span_.data(), span_.size() > 1 ? span_.size() - 1 : 0); + } + + gsl_api gsl_constexpr string_span_type ensure_z() const + { + return gsl::ensure_z(span_.data(), span_.size()); + } + + gsl_api gsl_constexpr czstring_type assume_z() const gsl_noexcept + { + return span_.data(); + } + + private: + span_type span_; + }; + + // + // zString types: + // + + typedef basic_zstring_span zstring_span; + typedef basic_zstring_span czstring_span; + +#if gsl_HAVE(WCHAR) + typedef basic_zstring_span wzstring_span; + typedef basic_zstring_span cwzstring_span; +#endif + +} // namespace gsl + +#if gsl_CPP11_OR_GREATER || gsl_COMPILER_MSVC_VERSION >= 120 + +namespace std +{ +template <> +struct hash +{ +public: + std::size_t operator()(gsl::byte v) const gsl_noexcept + { + return gsl::to_integer(v); + } +}; + +} // namespace std + +#endif + +gsl_RESTORE_MSVC_WARNINGS() + +#endif // GSL_GSL_LITE_HPP_INCLUDED + + // end of file diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_algorithm b/src/algorithms/libs/gsl/include/gsl/gsl_algorithm deleted file mode 100644 index c2ba31f30..000000000 --- a/src/algorithms/libs/gsl/include/gsl/gsl_algorithm +++ /dev/null @@ -1,61 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_ALGORITHM_H -#define GSL_ALGORITHM_H - -#include // for Expects -#include // for dynamic_extent, span - -#include // for copy_n -#include // for ptrdiff_t -#include // for is_assignable - -#ifdef _MSC_VER -#pragma warning(push) - -// turn off some warnings that are noisy about our Expects statements -#pragma warning(disable : 4127) // conditional expression is constant -#pragma warning(disable : 4996) // unsafe use of std::copy_n - -#endif // _MSC_VER - -namespace gsl -{ -// Note: this will generate faster code than std::copy using span iterator in older msvc+stl -// not necessary for msvc since VS2017 15.8 (_MSC_VER >= 1915) -template -void copy(span src, span dest) -{ - static_assert(std::is_assignable::value, - "Elements of source span can not be assigned to elements of destination span"); - static_assert(SrcExtent == dynamic_extent || DestExtent == dynamic_extent || - (SrcExtent <= DestExtent), - "Source range is longer than target range"); - - Expects(dest.size() >= src.size()); - GSL_SUPPRESS(stl.1) // NO-FORMAT: attribute - std::copy_n(src.data(), src.size(), dest.data()); -} - -} // namespace gsl - -#ifdef _MSC_VER -#pragma warning(pop) -#endif // _MSC_VER - -#endif // GSL_ALGORITHM_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_assert b/src/algorithms/libs/gsl/include/gsl/gsl_assert deleted file mode 100644 index c70463395..000000000 --- a/src/algorithms/libs/gsl/include/gsl/gsl_assert +++ /dev/null @@ -1,177 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_CONTRACTS_H -#define GSL_CONTRACTS_H - -#include -#include // for logic_error - -// -// make suppress attributes parse for some compilers -// Hopefully temporary until suppression standardization occurs -// -#if defined(__clang__) -#define GSL_SUPPRESS(x) [[gsl::suppress("x")]] -#else -#if defined(_MSC_VER) -#define GSL_SUPPRESS(x) [[gsl::suppress(x)]] -#else -#define GSL_SUPPRESS(x) -#endif // _MSC_VER -#endif // __clang__ - -// -// Temporary until MSVC STL supports no-exceptions mode. -// Currently terminate is a no-op in this mode, so we add termination behavior back -// -#if defined(_MSC_VER) && defined(_HAS_EXCEPTIONS) && !_HAS_EXCEPTIONS -#define GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND -#include -#define RANGE_CHECKS_FAILURE 0 - -#if defined(__clang__) -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Winvalid-noreturn" -#endif - -#endif - -// -// There are three configuration options for this GSL implementation's behavior -// when pre/post conditions on the GSL types are violated: -// -// 1. GSL_TERMINATE_ON_CONTRACT_VIOLATION: std::terminate will be called (default) -// 2. GSL_THROW_ON_CONTRACT_VIOLATION: a gsl::fail_fast exception will be thrown -// 3. GSL_UNENFORCED_ON_CONTRACT_VIOLATION: nothing happens -// -#if !(defined(GSL_THROW_ON_CONTRACT_VIOLATION) || defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) || \ - defined(GSL_UNENFORCED_ON_CONTRACT_VIOLATION)) -#define GSL_TERMINATE_ON_CONTRACT_VIOLATION -#endif - -#define GSL_STRINGIFY_DETAIL(x) #x -#define GSL_STRINGIFY(x) GSL_STRINGIFY_DETAIL(x) - -#if defined(__clang__) || defined(__GNUC__) -#define GSL_LIKELY(x) __builtin_expect(!!(x), 1) -#define GSL_UNLIKELY(x) __builtin_expect(!!(x), 0) -#else -#define GSL_LIKELY(x) (!!(x)) -#define GSL_UNLIKELY(x) (!!(x)) -#endif - -// -// GSL_ASSUME(cond) -// -// Tell the optimizer that the predicate cond must hold. It is unspecified -// whether or not cond is actually evaluated. -// -#ifdef _MSC_VER -#define GSL_ASSUME(cond) __assume(cond) -#elif defined(__GNUC__) -#define GSL_ASSUME(cond) ((cond) ? static_cast(0) : __builtin_unreachable()) -#else -#define GSL_ASSUME(cond) static_cast((cond) ? 0 : 0) -#endif - -// -// GSL.assert: assertions -// - -namespace gsl -{ -struct fail_fast : public std::logic_error -{ - explicit fail_fast(char const* const message) : std::logic_error(message) {} -}; - -namespace details -{ -#if defined(GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND) - - typedef void (__cdecl *terminate_handler)(); - - GSL_SUPPRESS(f.6) // NO-FORMAT: attribute - [[noreturn]] inline void __cdecl default_terminate_handler() - { - __fastfail(RANGE_CHECKS_FAILURE); - } - - inline gsl::details::terminate_handler& get_terminate_handler() noexcept - { - static terminate_handler handler = &default_terminate_handler; - return handler; - } - -#endif - - [[noreturn]] inline void terminate() noexcept - { -#if defined(GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND) - (*gsl::details::get_terminate_handler())(); -#else - std::terminate(); -#endif - } - -#if defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) - - template - [[noreturn]] void throw_exception(Exception&&) noexcept - { - gsl::details::terminate(); - } - -#else - - template - [[noreturn]] void throw_exception(Exception&& exception) - { - throw std::forward(exception); - } - -#endif // GSL_TERMINATE_ON_CONTRACT_VIOLATION - -} // namespace details -} // namespace gsl - -#if defined(GSL_THROW_ON_CONTRACT_VIOLATION) - -#define GSL_CONTRACT_CHECK(type, cond) \ - (GSL_LIKELY(cond) ? static_cast(0) \ - : gsl::details::throw_exception(gsl::fail_fast( \ - "GSL: " type " failure at " __FILE__ ": " GSL_STRINGIFY(__LINE__)))) - -#elif defined(GSL_TERMINATE_ON_CONTRACT_VIOLATION) - -#define GSL_CONTRACT_CHECK(type, cond) \ - (GSL_LIKELY(cond) ? static_cast(0) : gsl::details::terminate()) - -#elif defined(GSL_UNENFORCED_ON_CONTRACT_VIOLATION) - -#define GSL_CONTRACT_CHECK(type, cond) GSL_ASSUME(cond) - -#endif // GSL_THROW_ON_CONTRACT_VIOLATION - -#define Expects(cond) GSL_CONTRACT_CHECK("Precondition", cond) -#define Ensures(cond) GSL_CONTRACT_CHECK("Postcondition", cond) - -#if defined(GSL_MSVC_USE_STL_NOEXCEPTION_WORKAROUND) && defined(__clang__) -#pragma clang diagnostic pop -#endif - -#endif // GSL_CONTRACTS_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_byte b/src/algorithms/libs/gsl/include/gsl/gsl_byte deleted file mode 100644 index 861446dc5..000000000 --- a/src/algorithms/libs/gsl/include/gsl/gsl_byte +++ /dev/null @@ -1,203 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_BYTE_H -#define GSL_BYTE_H - -// -// make suppress attributes work for some compilers -// Hopefully temporary until suppression standardization occurs -// -#if defined(__clang__) -#define GSL_SUPPRESS(x) [[gsl::suppress("x")]] -#else -#if defined(_MSC_VER) -#define GSL_SUPPRESS(x) [[gsl::suppress(x)]] -#else -#define GSL_SUPPRESS(x) -#endif // _MSC_VER -#endif // __clang__ - -#include - -#ifdef _MSC_VER - -#pragma warning(push) - -// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. -#pragma warning(disable : 26493) // don't use c-style casts // TODO: MSVC suppression in templates does not always work - -#ifndef GSL_USE_STD_BYTE -// this tests if we are under MSVC and the standard lib has std::byte and it is enabled -#if defined(_HAS_STD_BYTE) && _HAS_STD_BYTE - -#define GSL_USE_STD_BYTE 1 - -#else // defined(_HAS_STD_BYTE) && _HAS_STD_BYTE - -#define GSL_USE_STD_BYTE 0 - -#endif // defined(_HAS_STD_BYTE) && _HAS_STD_BYTE -#endif // GSL_USE_STD_BYTE - -#else // _MSC_VER - -#ifndef GSL_USE_STD_BYTE -// this tests if we are under GCC or Clang with enough -std:c++1z power to get us std::byte -// also check if libc++ version is sufficient (> 5.0) or libstc++ actually contains std::byte -#if defined(__cplusplus) && (__cplusplus >= 201703L) && \ - (defined(__cpp_lib_byte) && (__cpp_lib_byte >= 201603) || \ - defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 5000)) - -#define GSL_USE_STD_BYTE 1 -#include - -#else // defined(__cplusplus) && (__cplusplus >= 201703L) && - // (defined(__cpp_lib_byte) && (__cpp_lib_byte >= 201603) || - // defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 5000)) - -#define GSL_USE_STD_BYTE 0 - -#endif //defined(__cplusplus) && (__cplusplus >= 201703L) && - // (defined(__cpp_lib_byte) && (__cpp_lib_byte >= 201603) || - // defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 5000)) -#endif // GSL_USE_STD_BYTE - -#endif // _MSC_VER - -// Use __may_alias__ attribute on gcc and clang -#if defined __clang__ || (defined(__GNUC__) && __GNUC__ > 5) -#define byte_may_alias __attribute__((__may_alias__)) -#else // defined __clang__ || defined __GNUC__ -#define byte_may_alias -#endif // defined __clang__ || defined __GNUC__ - -namespace gsl -{ -#if GSL_USE_STD_BYTE - -using std::byte; -using std::to_integer; - -#else // GSL_USE_STD_BYTE - -// This is a simple definition for now that allows -// use of byte within span<> to be standards-compliant -enum class byte_may_alias byte : unsigned char -{ -}; - -template ::value>> -constexpr byte& operator<<=(byte& b, IntegerType shift) noexcept -{ - return b = byte(static_cast(b) << shift); -} - -template ::value>> -constexpr byte operator<<(byte b, IntegerType shift) noexcept -{ - return byte(static_cast(b) << shift); -} - -template ::value>> -constexpr byte& operator>>=(byte& b, IntegerType shift) noexcept -{ - return b = byte(static_cast(b) >> shift); -} - -template ::value>> -constexpr byte operator>>(byte b, IntegerType shift) noexcept -{ - return byte(static_cast(b) >> shift); -} - -constexpr byte& operator|=(byte& l, byte r) noexcept -{ - return l = byte(static_cast(l) | static_cast(r)); -} - -constexpr byte operator|(byte l, byte r) noexcept -{ - return byte(static_cast(l) | static_cast(r)); -} - -constexpr byte& operator&=(byte& l, byte r) noexcept -{ - return l = byte(static_cast(l) & static_cast(r)); -} - -constexpr byte operator&(byte l, byte r) noexcept -{ - return byte(static_cast(l) & static_cast(r)); -} - -constexpr byte& operator^=(byte& l, byte r) noexcept -{ - return l = byte(static_cast(l) ^ static_cast(r)); -} - -constexpr byte operator^(byte l, byte r) noexcept -{ - return byte(static_cast(l) ^ static_cast(r)); -} - -constexpr byte operator~(byte b) noexcept { return byte(~static_cast(b)); } - -template ::value>> -constexpr IntegerType to_integer(byte b) noexcept -{ - return static_cast(b); -} - -#endif // GSL_USE_STD_BYTE - -template -constexpr byte to_byte_impl(T t) noexcept -{ - static_assert( - E, "gsl::to_byte(t) must be provided an unsigned char, otherwise data loss may occur. " - "If you are calling to_byte with an integer contant use: gsl::to_byte() version."); - return static_cast(t); -} -template <> -// NOTE: need suppression since c++14 does not allow "return {t}" -// GSL_SUPPRESS(type.4) // NO-FORMAT: attribute // TODO: suppression does not work -constexpr byte to_byte_impl(unsigned char t) noexcept -{ - return byte(t); -} - -template -constexpr byte to_byte(T t) noexcept -{ - return to_byte_impl::value, T>(t); -} - -template -constexpr byte to_byte() noexcept -{ - static_assert(I >= 0 && I <= 255, - "gsl::byte only has 8 bits of storage, values must be in range 0-255"); - return static_cast(I); -} - -} // namespace gsl - -#ifdef _MSC_VER -#pragma warning(pop) -#endif // _MSC_VER - -#endif // GSL_BYTE_H diff --git a/src/algorithms/libs/gsl/include/gsl/gsl_util b/src/algorithms/libs/gsl/include/gsl/gsl_util deleted file mode 100644 index 4addde6ad..000000000 --- a/src/algorithms/libs/gsl/include/gsl/gsl_util +++ /dev/null @@ -1,175 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_UTIL_H -#define GSL_UTIL_H - -#include // for Expects - -#include -#include // for ptrdiff_t, size_t -#include // for exception -#include // for initializer_list -#include // for is_signed, integral_constant -#include // for forward - -#if defined(_MSC_VER) && !defined(__clang__) - -#pragma warning(push) -#pragma warning(disable : 4127) // conditional expression is constant - -#if _MSC_VER < 1910 -#pragma push_macro("constexpr") -#define constexpr /*constexpr*/ -#endif // _MSC_VER < 1910 -#endif // _MSC_VER - -#if (defined(_MSC_VER) && _MSC_VER < 1910) || (!defined(__clang__) && defined(__GNUC__) && __GUNC__ < 6) -#define GSL_CONSTEXPR_NARROW 0 -#else -#define GSL_CONSTEXPR_NARROW 1 -#endif - -namespace gsl -{ -// -// GSL.util: utilities -// - -// index type for all container indexes/subscripts/sizes -using index = std::ptrdiff_t; - -// final_action allows you to ensure something gets run at the end of a scope -template -class final_action -{ -public: - explicit final_action(F f) noexcept : f_(std::move(f)) {} - - final_action(final_action&& other) noexcept : f_(std::move(other.f_)), invoke_(other.invoke_) - { - other.invoke_ = false; - } - - final_action(const final_action&) = delete; - final_action& operator=(const final_action&) = delete; - final_action& operator=(final_action&&) = delete; - - GSL_SUPPRESS(f.6) // NO-FORMAT: attribute // terminate if throws - ~final_action() noexcept - { - if (invoke_) f_(); - } - -private: - F f_; - bool invoke_{true}; -}; - -// finally() - convenience function to generate a final_action -template -final_action finally(const F& f) noexcept -{ - return final_action(f); -} - -template -final_action finally(F&& f) noexcept -{ - return final_action(std::forward(f)); -} - -// narrow_cast(): a searchable way to do narrowing casts of values -template -GSL_SUPPRESS(type.1) // NO-FORMAT: attribute -constexpr T narrow_cast(U&& u) noexcept -{ - return static_cast(std::forward(u)); -} - -struct narrowing_error : public std::exception -{ -}; - -namespace details -{ - template - struct is_same_signedness - : public std::integral_constant::value == std::is_signed::value> - { - }; -} // namespace details - -// narrow() : a checked version of narrow_cast() that throws if the cast changed the value -template -GSL_SUPPRESS(type.1) // NO-FORMAT: attribute -GSL_SUPPRESS(f.6) // NO-FORMAT: attribute // TODO: MSVC /analyze does not recognise noexcept(false) -#if GSL_CONSTEXPR_NARROW -constexpr -#endif -T narrow(U u) noexcept(false) -{ - T t = narrow_cast(u); - if (static_cast(t) != u) gsl::details::throw_exception(narrowing_error()); - if (!details::is_same_signedness::value && ((t < T{}) != (u < U{}))) - gsl::details::throw_exception(narrowing_error()); - return t; -} - -// -// at() - Bounds-checked way of accessing builtin arrays, std::array, std::vector -// -template -GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute -GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute -constexpr T& at(T (&arr)[N], const index i) -{ - Expects(i >= 0 && i < narrow_cast(N)); - return arr[narrow_cast(i)]; -} - -template -GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute -GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute -constexpr auto at(Cont& cont, const index i) -> decltype(cont[cont.size()]) -{ - Expects(i >= 0 && i < narrow_cast(cont.size())); - using size_type = decltype(cont.size()); - return cont[narrow_cast(i)]; -} - -template -GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute -constexpr T at(const std::initializer_list cont, const index i) -{ - Expects(i >= 0 && i < narrow_cast(cont.size())); - return *(cont.begin() + i); -} - -} // namespace gsl - -#if defined(_MSC_VER) && !defined(__clang__) -#if _MSC_VER < 1910 -#undef constexpr -#pragma pop_macro("constexpr") - -#endif // _MSC_VER < 1910 - -#pragma warning(pop) - -#endif // _MSC_VER - -#endif // GSL_UTIL_H diff --git a/src/algorithms/libs/gsl/include/gsl/multi_span b/src/algorithms/libs/gsl/include/gsl/multi_span deleted file mode 100644 index 0c1506ed4..000000000 --- a/src/algorithms/libs/gsl/include/gsl/multi_span +++ /dev/null @@ -1,2293 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_MULTI_SPAN_H -#define GSL_MULTI_SPAN_H - -#include // for Expects -#include // for byte -#include // for narrow_cast - -#include // for transform, lexicographical_compare -#include // for array -#include -#include // for ptrdiff_t, size_t, nullptr_t -#include // for PTRDIFF_MAX -#include // for divides, multiplies, minus, negate, plus -#include // for initializer_list -#include // for iterator, random_access_iterator_tag -#include // for numeric_limits -#include -#include -#include -#include // for basic_string -#include // for enable_if_t, remove_cv_t, is_same, is_co... -#include - -#if defined(_MSC_VER) && !defined(__clang__) - -// turn off some warnings that are noisy about our Expects statements -#pragma warning(push) -#pragma warning(disable : 4127) // conditional expression is constant -#pragma warning(disable : 4702) // unreachable code - -// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. -#pragma warning(disable : 26495) // uninitalized member when constructor calls constructor -#pragma warning(disable : 26473) // in some instantiations we cast to the same type -#pragma warning(disable : 26490) // TODO: bug in parser - attributes and templates -#pragma warning(disable : 26465) // TODO: bug - suppression does not work on template functions - -#if _MSC_VER < 1910 -#pragma push_macro("constexpr") -#define constexpr /*constexpr*/ - -#endif // _MSC_VER < 1910 -#endif // _MSC_VER - -// GCC 7 does not like the signed unsigned missmatch (size_t ptrdiff_t) -// While there is a conversion from signed to unsigned, it happens at -// compiletime, so the compiler wouldn't have to warn indiscriminently, but -// could check if the source value actually doesn't fit into the target type -// and only warn in those cases. -#if defined(__GNUC__) && __GNUC__ > 6 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wsign-conversion" -#endif - -namespace gsl -{ - -/* -** begin definitions of index and bounds -*/ -namespace details -{ - template - struct SizeTypeTraits - { - static const SizeType max_value = std::numeric_limits::max(); - }; - - template - class are_integral : public std::integral_constant - { - }; - - template - class are_integral - : public std::integral_constant::value && are_integral::value> - { - }; -} // namespace details - -template -class multi_span_index final -{ - static_assert(Rank > 0, "Rank must be greater than 0!"); - - template - friend class multi_span_index; - -public: - static const std::size_t rank = Rank; - using value_type = std::ptrdiff_t; - using size_type = value_type; - using reference = std::add_lvalue_reference_t; - using const_reference = std::add_lvalue_reference_t>; - - constexpr multi_span_index() noexcept {} - - constexpr multi_span_index(const value_type (&values)[Rank]) noexcept - { - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute - std::copy(values, values + Rank, elems); - } - - template ::value>> - constexpr multi_span_index(Ts... ds) noexcept : elems{narrow_cast(ds)...} - {} - - constexpr multi_span_index(const multi_span_index& other) noexcept = default; - - constexpr multi_span_index& operator=(const multi_span_index& rhs) noexcept = default; - - // Preconditions: component_idx < rank - GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr reference operator[](std::size_t component_idx) - { - Expects(component_idx < Rank); // Component index must be less than rank - return elems[component_idx]; - } - - // Preconditions: component_idx < rank - GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr const_reference operator[](std::size_t component_idx) const - { - Expects(component_idx < Rank); // Component index must be less than rank - return elems[component_idx]; - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute - constexpr bool operator==(const multi_span_index& rhs) const - { - return std::equal(elems, elems + rank, rhs.elems); - } - - constexpr bool operator!=(const multi_span_index& rhs) const - { - return !(*this == rhs); - } - - constexpr multi_span_index operator+() const noexcept { return *this; } - - constexpr multi_span_index operator-() const - { - multi_span_index ret = *this; - std::transform(ret, ret + rank, ret, std::negate{}); - return ret; - } - - constexpr multi_span_index operator+(const multi_span_index& rhs) const - { - multi_span_index ret = *this; - ret += rhs; - return ret; - } - - constexpr multi_span_index operator-(const multi_span_index& rhs) const - { - multi_span_index ret = *this; - ret -= rhs; - return ret; - } - - constexpr multi_span_index& operator+=(const multi_span_index& rhs) - { - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute - std::transform(elems, elems + rank, rhs.elems, elems, - std::plus{}); - return *this; - } - - constexpr multi_span_index& operator-=(const multi_span_index& rhs) - { - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute - std::transform(elems, elems + rank, rhs.elems, elems, std::minus{}); - return *this; - } - - constexpr multi_span_index operator*(value_type v) const - { - multi_span_index ret = *this; - ret *= v; - return ret; - } - - constexpr multi_span_index operator/(value_type v) const - { - multi_span_index ret = *this; - ret /= v; - return ret; - } - - friend constexpr multi_span_index operator*(value_type v, const multi_span_index& rhs) - { - return rhs * v; - } - - constexpr multi_span_index& operator*=(value_type v) - { - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute - std::transform(elems, elems + rank, elems, - [v](value_type x) { return std::multiplies{}(x, v); }); - return *this; - } - - constexpr multi_span_index& operator/=(value_type v) - { - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.3) // NO-FORMAT: attribute - std::transform(elems, elems + rank, elems, - [v](value_type x) { return std::divides{}(x, v); }); - return *this; - } - -private: - value_type elems[Rank] = {}; -}; - -#if !defined(_MSC_VER) || _MSC_VER >= 1910 - -struct static_bounds_dynamic_range_t -{ - template ::value>> - constexpr operator T() const noexcept - { - return narrow_cast(-1); - } -}; - -constexpr bool operator==(static_bounds_dynamic_range_t, static_bounds_dynamic_range_t) noexcept -{ - return true; -} - -constexpr bool operator!=(static_bounds_dynamic_range_t, static_bounds_dynamic_range_t) noexcept -{ - return false; -} - -template ::value>> -constexpr bool operator==(static_bounds_dynamic_range_t, T other) noexcept -{ - return narrow_cast(-1) == other; -} - -template ::value>> -constexpr bool operator==(T left, static_bounds_dynamic_range_t right) noexcept -{ - return right == left; -} - -template ::value>> -constexpr bool operator!=(static_bounds_dynamic_range_t, T other) noexcept -{ - return narrow_cast(-1) != other; -} - -template ::value>> -constexpr bool operator!=(T left, static_bounds_dynamic_range_t right) noexcept -{ - return right != left; -} - -constexpr static_bounds_dynamic_range_t dynamic_range{}; -#else -const std::ptrdiff_t dynamic_range = -1; -#endif - -struct generalized_mapping_tag -{ -}; -struct contiguous_mapping_tag : generalized_mapping_tag -{ -}; - -namespace details -{ - - template - struct LessThan - { - static const bool value = Left < Right; - }; - - template - struct BoundsRanges - { - using size_type = std::ptrdiff_t; - static const size_type Depth = 0; - static const size_type DynamicNum = 0; - static const size_type CurrentRange = 1; - static const size_type TotalSize = 1; - - // TODO : following signature is for work around VS bug - template - constexpr BoundsRanges(const OtherRange&, bool /* firstLevel */) - {} - - constexpr BoundsRanges(const std::ptrdiff_t* const) {} - constexpr BoundsRanges() noexcept = default; - - template - constexpr void serialize(T&) const - {} - - template - constexpr size_type linearize(const T&) const - { - return 0; - } - - template - constexpr size_type contains(const T&) const - { - return -1; - } - - constexpr size_type elementNum(std::size_t) const noexcept { return 0; } - - constexpr size_type totalSize() const noexcept { return TotalSize; } - - constexpr bool operator==(const BoundsRanges&) const noexcept { return true; } - }; - - template - struct BoundsRanges : BoundsRanges - { - using Base = BoundsRanges; - using size_type = std::ptrdiff_t; - static const std::size_t Depth = Base::Depth + 1; - static const std::size_t DynamicNum = Base::DynamicNum + 1; - static const size_type CurrentRange = dynamic_range; - static const size_type TotalSize = dynamic_range; - - private: - size_type m_bound; - - public: - GSL_SUPPRESS(f.23) // NO-FORMAT: attribute // this pointer type is cannot be assigned nullptr - issue in not_null - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr BoundsRanges(const std::ptrdiff_t* const arr) - : Base(arr + 1), m_bound(*arr * this->Base::totalSize()) - { - Expects(0 <= *arr); - } - - constexpr BoundsRanges() noexcept : m_bound(0) {} - - template - constexpr BoundsRanges(const BoundsRanges& other, - bool /* firstLevel */ = true) - : Base(static_cast&>(other), false) - , m_bound(other.totalSize()) - {} - - template - constexpr void serialize(T& arr) const - { - arr[Dim] = elementNum(); - this->Base::template serialize(arr); - } - - template - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr size_type linearize(const T& arr) const - { - const size_type index = this->Base::totalSize() * arr[Dim]; - Expects(index < m_bound); - return index + this->Base::template linearize(arr); - } - - template - constexpr size_type contains(const T& arr) const - { - const ptrdiff_t last = this->Base::template contains(arr); - if (last == -1) return -1; - const ptrdiff_t cur = this->Base::totalSize() * arr[Dim]; - return cur < m_bound ? cur + last : -1; - } - - GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used - constexpr size_type totalSize() const noexcept - { - return m_bound; - } - - GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used - constexpr size_type elementNum() const noexcept - { - return totalSize() / this->Base::totalSize(); - } - - GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used - constexpr size_type elementNum(std::size_t dim) const noexcept - { - if (dim > 0) - return this->Base::elementNum(dim - 1); - else - return elementNum(); - } - - constexpr bool operator==(const BoundsRanges& rhs) const noexcept - { - return m_bound == rhs.m_bound && - static_cast(*this) == static_cast(rhs); - } - }; - - template - struct BoundsRanges : BoundsRanges - { - using Base = BoundsRanges; - using size_type = std::ptrdiff_t; - static const std::size_t Depth = Base::Depth + 1; - static const std::size_t DynamicNum = Base::DynamicNum; - static const size_type CurrentRange = CurRange; - static const size_type TotalSize = - Base::TotalSize == dynamic_range ? dynamic_range : CurrentRange * Base::TotalSize; - - constexpr BoundsRanges(const std::ptrdiff_t* const arr) : Base(arr) {} - constexpr BoundsRanges() = default; - - template - constexpr BoundsRanges(const BoundsRanges& other, - bool firstLevel = true) - : Base(static_cast&>(other), false) - { - GSL_SUPPRESS(type.4) // NO-FORMAT: attribute // TODO: false positive - (void) firstLevel; - } - - template - constexpr void serialize(T& arr) const - { - arr[Dim] = elementNum(); - this->Base::template serialize(arr); - } - - template - constexpr size_type linearize(const T& arr) const - { - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - Expects(arr[Dim] >= 0 && arr[Dim] < CurrentRange); // Index is out of range - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - const ptrdiff_t d = arr[Dim]; - return this->Base::totalSize() * d + - this->Base::template linearize(arr); - } - - template - constexpr size_type contains(const T& arr) const - { - if (arr[Dim] >= CurrentRange) return -1; - const size_type last = this->Base::template contains(arr); - if (last == -1) return -1; - return this->Base::totalSize() * arr[Dim] + last; - } - - GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used - constexpr size_type totalSize() const noexcept - { - return CurrentRange * this->Base::totalSize(); - } - - GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used - constexpr size_type elementNum() const noexcept - { - return CurrentRange; - } - - GSL_SUPPRESS(c.128) // NO-FORMAT: attribute // no pointers to BoundsRanges should be ever used - constexpr size_type elementNum(std::size_t dim) const noexcept - { - if (dim > 0) - return this->Base::elementNum(dim - 1); - else - return elementNum(); - } - - constexpr bool operator==(const BoundsRanges& rhs) const noexcept - { - return static_cast(*this) == static_cast(rhs); - } - }; - - template - struct BoundsRangeConvertible - : public std::integral_constant= TargetType::TotalSize || - TargetType::TotalSize == dynamic_range || - SourceType::TotalSize == dynamic_range || - TargetType::TotalSize == 0)> - { - }; - - template - struct TypeListIndexer - { - const TypeChain& obj_; - constexpr TypeListIndexer(const TypeChain& obj) : obj_(obj) {} - - template - constexpr const TypeChain& getObj(std::true_type) - { - return obj_; - } - - template - constexpr auto getObj(std::false_type) - -> decltype(TypeListIndexer(static_cast(obj_)).template get()) - { - return TypeListIndexer(static_cast(obj_)).template get(); - } - - template - constexpr auto get() -> decltype(getObj(std::integral_constant())) - { - return getObj(std::integral_constant()); - } - }; - - template - constexpr TypeListIndexer createTypeListIndexer(const TypeChain& obj) - { - return TypeListIndexer(obj); - } - - template 1), - typename Ret = std::enable_if_t>> - constexpr Ret shift_left(const multi_span_index& other) noexcept - { - Ret ret{}; - for (std::size_t i = 0; i < Rank - 1; ++i) - { - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - ret[i] = other[i + 1]; - } - return ret; - } -} // namespace details - -template -class bounds_iterator; - -template -class static_bounds -{ -public: - static_bounds(const details::BoundsRanges&) {} -}; - -template -class static_bounds -{ - using MyRanges = details::BoundsRanges; - - MyRanges m_ranges; - constexpr static_bounds(const MyRanges& range) noexcept : m_ranges(range) {} - - template - friend class static_bounds; - -public: - static const std::size_t rank = MyRanges::Depth; - static const std::size_t dynamic_rank = MyRanges::DynamicNum; - static const std::ptrdiff_t static_size = MyRanges::TotalSize; - - using size_type = std::ptrdiff_t; - using index_type = multi_span_index; - using const_index_type = std::add_const_t; - using iterator = bounds_iterator; - using const_iterator = bounds_iterator; - using difference_type = std::ptrdiff_t; - using sliced_type = static_bounds; - using mapping_type = contiguous_mapping_tag; - - constexpr static_bounds() /*noexcept*/ = default; - - template - struct BoundsRangeConvertible2; - - template > - static auto helpBoundsRangeConvertible(SourceType, TargetType, std::true_type) -> Ret; - - template - static auto helpBoundsRangeConvertible(SourceType, TargetType, ...) -> std::false_type; - - template - struct BoundsRangeConvertible2 - : decltype(helpBoundsRangeConvertible( - SourceType(), TargetType(), - std::integral_constant())) - { - }; - - template - struct BoundsRangeConvertible2 : std::true_type - { - }; - - template - struct BoundsRangeConvertible - : decltype(helpBoundsRangeConvertible( - SourceType(), TargetType(), - std::integral_constant::value || - TargetType::CurrentRange == dynamic_range || - SourceType::CurrentRange == dynamic_range)>())) - { - }; - - template - struct BoundsRangeConvertible : std::true_type - { - }; - - template , - details::BoundsRanges>::value>> - constexpr static_bounds(const static_bounds& other) : m_ranges(other.m_ranges) - { - Expects((MyRanges::DynamicNum == 0 && details::BoundsRanges::DynamicNum == 0) || - MyRanges::DynamicNum > 0 || other.m_ranges.totalSize() >= m_ranges.totalSize()); - } - - constexpr static_bounds(std::initializer_list il) : m_ranges(il.begin()) - { - // Size of the initializer list must match the rank of the array - Expects((MyRanges::DynamicNum == 0 && il.size() == 1 && *il.begin() == static_size) || - MyRanges::DynamicNum == il.size()); - // Size of the range must be less than the max element of the size type - Expects(m_ranges.totalSize() <= PTRDIFF_MAX); - } - - constexpr sliced_type slice() const noexcept - { - return sliced_type{static_cast&>(m_ranges)}; - } - - constexpr size_type stride() const noexcept { return rank > 1 ? slice().size() : 1; } - - constexpr size_type size() const noexcept { return m_ranges.totalSize(); } - - constexpr size_type total_size() const noexcept { return m_ranges.totalSize(); } - - constexpr size_type linearize(const index_type& idx) const { return m_ranges.linearize(idx); } - - constexpr bool contains(const index_type& idx) const noexcept - { - return m_ranges.contains(idx) != -1; - } - - constexpr size_type operator[](std::size_t idx) const noexcept - { - return m_ranges.elementNum(idx); - } - - template - constexpr size_type extent() const noexcept - { - static_assert(Dim < rank, - "dimension should be less than rank (dimension count starts from 0)"); - return details::createTypeListIndexer(m_ranges).template get().elementNum(); - } - - template - constexpr size_type extent(IntType dim) const - { - static_assert(std::is_integral::value, - "Dimension parameter must be supplied as an integral type."); - auto real_dim = narrow_cast(dim); - Expects(real_dim < rank); - - return m_ranges.elementNum(real_dim); - } - - constexpr index_type index_bounds() const noexcept - { - size_type extents[rank] = {}; - m_ranges.serialize(extents); - return {extents}; - } - - template - constexpr bool operator==(const static_bounds& rhs) const noexcept - { - return this->size() == rhs.size(); - } - - template - constexpr bool operator!=(const static_bounds& rhs) const noexcept - { - return !(*this == rhs); - } - - constexpr const_iterator begin() const noexcept - { - return const_iterator(*this, index_type{}); - } - - constexpr const_iterator end() const noexcept - { - return const_iterator(*this, this->index_bounds()); - } -}; - -template -class strided_bounds { - template - friend class strided_bounds; - -public: - static const std::size_t rank = Rank; - using value_type = std::ptrdiff_t; - using reference = std::add_lvalue_reference_t; - using const_reference = std::add_const_t; - using size_type = value_type; - using difference_type = value_type; - using index_type = multi_span_index; - using const_index_type = std::add_const_t; - using iterator = bounds_iterator; - using const_iterator = bounds_iterator; - static const value_type dynamic_rank = rank; - static const value_type static_size = dynamic_range; - using sliced_type = std::conditional_t, void>; - using mapping_type = generalized_mapping_tag; - - constexpr strided_bounds(const strided_bounds&) noexcept = default; - - constexpr strided_bounds& operator=(const strided_bounds&) noexcept = default; - - constexpr strided_bounds(const value_type (&values)[rank], index_type strides) - : m_extents(values), m_strides(std::move(strides)) - {} - - constexpr strided_bounds(const index_type& extents, const index_type& strides) noexcept - : m_extents(extents), m_strides(strides) - { - } - - constexpr index_type strides() const noexcept { return m_strides; } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr size_type total_size() const noexcept - { - size_type ret = 0; - for (std::size_t i = 0; i < rank; ++i) { ret += (m_extents[i] - 1) * m_strides[i]; } - return ret + 1; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr size_type size() const noexcept - { - size_type ret = 1; - for (std::size_t i = 0; i < rank; ++i) { ret *= m_extents[i]; } - return ret; - } - - constexpr bool contains(const index_type& idx) const noexcept - { - for (std::size_t i = 0; i < rank; ++i) - { - if (idx[i] < 0 || idx[i] >= m_extents[i]) return false; - } - return true; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr size_type linearize(const index_type& idx) const - { - size_type ret = 0; - for (std::size_t i = 0; i < rank; i++) - { - Expects(idx[i] < m_extents[i]); // index is out of bounds of the array - ret += idx[i] * m_strides[i]; - } - return ret; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr size_type stride() const noexcept { return m_strides[0]; } - - template 1), typename Ret = std::enable_if_t> - constexpr sliced_type slice() const - { - return {details::shift_left(m_extents), details::shift_left(m_strides)}; - } - - template - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr size_type extent() const noexcept - { - static_assert(Dim < Rank, - "dimension should be less than rank (dimension count starts from 0)"); - return m_extents[Dim]; - } - - constexpr index_type index_bounds() const noexcept { return m_extents; } - - constexpr const_iterator begin() const noexcept { return const_iterator{*this, index_type{}}; } - - constexpr const_iterator end() const noexcept { return const_iterator{*this, index_bounds()}; } - -private: - index_type m_extents; - index_type m_strides; -}; - -template -struct is_bounds : std::integral_constant -{ -}; -template -struct is_bounds> : std::integral_constant -{ -}; -template -struct is_bounds> : std::integral_constant -{ -}; - -template -class bounds_iterator -{ -public: - static const std::size_t rank = IndexType::rank; - using iterator_category = std::random_access_iterator_tag; - using value_type = IndexType; - using difference_type = std::ptrdiff_t; - using pointer = value_type*; - using reference = value_type&; - using index_type = value_type; - using index_size_type = typename IndexType::value_type; - template - explicit bounds_iterator(const Bounds& bnd, value_type curr) noexcept - : boundary_(bnd.index_bounds()), curr_(std::move(curr)) - { - static_assert(is_bounds::value, "Bounds type must be provided"); - } - - constexpr reference operator*() const noexcept { return curr_; } - - constexpr pointer operator->() const noexcept { return &curr_; } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute - constexpr bounds_iterator& operator++() noexcept - - { - for (std::size_t i = rank; i-- > 0;) - { - if (curr_[i] < boundary_[i] - 1) - { - curr_[i]++; - return *this; - } - curr_[i] = 0; - } - // If we're here we've wrapped over - set to past-the-end. - curr_ = boundary_; - return *this; - } - - constexpr bounds_iterator operator++(int) noexcept - { - auto ret = *this; - ++(*this); - return ret; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr bounds_iterator& operator--() - { - if (!less(curr_, boundary_)) - { - // if at the past-the-end, set to last element - for (std::size_t i = 0; i < rank; ++i) { curr_[i] = boundary_[i] - 1; } - return *this; - } - for (std::size_t i = rank; i-- > 0;) - { - if (curr_[i] >= 1) - { - curr_[i]--; - return *this; - } - curr_[i] = boundary_[i] - 1; - } - // If we're here the preconditions were violated - // "pre: there exists s such that r == ++s" - Expects(false); - return *this; - } - - constexpr bounds_iterator operator--(int) noexcept - { - auto ret = *this; - --(*this); - return ret; - } - - constexpr bounds_iterator operator+(difference_type n) const noexcept - { - bounds_iterator ret{*this}; - return ret += n; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr bounds_iterator& operator+=(difference_type n) - { - auto linear_idx = linearize(curr_) + n; - std::remove_const_t stride = 0; - stride[rank - 1] = 1; - for (std::size_t i = rank - 1; i-- > 0;) { stride[i] = stride[i + 1] * boundary_[i + 1]; } - for (std::size_t i = 0; i < rank; ++i) - { - curr_[i] = linear_idx / stride[i]; - linear_idx = linear_idx % stride[i]; - } - // index is out of bounds of the array - Expects(!less(curr_, index_type{}) && !less(boundary_, curr_)); - return *this; - } - - constexpr bounds_iterator operator-(difference_type n) const noexcept - { - bounds_iterator ret{*this}; - return ret -= n; - } - - constexpr bounds_iterator& operator-=(difference_type n) noexcept { return *this += -n; } - - constexpr difference_type operator-(const bounds_iterator& rhs) const noexcept - { - return linearize(curr_) - linearize(rhs.curr_); - } - - constexpr value_type operator[](difference_type n) const noexcept { return *(*this + n); } - - constexpr bool operator==(const bounds_iterator& rhs) const noexcept - { - return curr_ == rhs.curr_; - } - - - constexpr bool operator!=(const bounds_iterator& rhs) const noexcept { return !(*this == rhs); } - - constexpr bool operator<(const bounds_iterator& rhs) const noexcept - { - return less(curr_, rhs.curr_); - } - - constexpr bool operator<=(const bounds_iterator& rhs) const noexcept { return !(rhs < *this); } - - constexpr bool operator>(const bounds_iterator& rhs) const noexcept { return rhs < *this; } - - constexpr bool operator>=(const bounds_iterator& rhs) const noexcept { return !(rhs > *this); } - - void swap(bounds_iterator& rhs) noexcept - { - std::swap(boundary_, rhs.boundary_); - std::swap(curr_, rhs.curr_); - } - -private: - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr bool less(index_type& one, index_type& other) const noexcept - { - for (std::size_t i = 0; i < rank; ++i) - { - if (one[i] < other[i]) return true; - } - return false; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - constexpr index_size_type linearize(const value_type& idx) const noexcept - { - // TODO: Smarter impl. - // Check if past-the-end - index_size_type multiplier = 1; - index_size_type res = 0; - if (!less(idx, boundary_)) - { - res = 1; - for (std::size_t i = rank; i-- > 0;) - { - res += (idx[i] - 1) * multiplier; - multiplier *= boundary_[i]; - } - } - else - { - for (std::size_t i = rank; i-- > 0;) - { - res += idx[i] * multiplier; - multiplier *= boundary_[i]; - } - } - return res; - } - - value_type boundary_; - std::remove_const_t curr_; -}; - -template -bounds_iterator operator+(typename bounds_iterator::difference_type n, - const bounds_iterator& rhs) noexcept -{ - return rhs + n; -} - -namespace details -{ - template - constexpr std::enable_if_t< - std::is_same::value, - typename Bounds::index_type> - make_stride(const Bounds& bnd) noexcept - { - return bnd.strides(); - } - - // Make a stride vector from bounds, assuming contiguous memory. - template - constexpr std::enable_if_t< - std::is_same::value, - typename Bounds::index_type> - make_stride(const Bounds& bnd) noexcept - { - auto extents = bnd.index_bounds(); - typename Bounds::size_type stride[Bounds::rank] = {}; - - stride[Bounds::rank - 1] = 1; - for (std::size_t i = 1; i < Bounds::rank; ++i) - { - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - GSL_SUPPRESS(bounds.2) // NO-FORMAT: attribute - stride[Bounds::rank - i - 1] = stride[Bounds::rank - i] * extents[Bounds::rank - i]; - } - return {stride}; - } - - template - void verifyBoundsReshape(const BoundsSrc& src, const BoundsDest& dest) - { - static_assert(is_bounds::value && is_bounds::value, - "The src type and dest type must be bounds"); - static_assert(std::is_same::value, - "The source type must be a contiguous bounds"); - static_assert(BoundsDest::static_size == dynamic_range || - BoundsSrc::static_size == dynamic_range || - BoundsDest::static_size == BoundsSrc::static_size, - "The source bounds must have same size as dest bounds"); - Expects(src.size() == dest.size()); - } - -} // namespace details - -template -class contiguous_span_iterator; -template -class general_span_iterator; - -template -struct dim_t -{ - static const std::ptrdiff_t value = DimSize; -}; -template <> -struct dim_t -{ - static const std::ptrdiff_t value = dynamic_range; - const std::ptrdiff_t dvalue; - constexpr dim_t(std::ptrdiff_t size) noexcept : dvalue(size) {} -}; - -template = 0)>> -constexpr dim_t dim() noexcept -{ - return dim_t(); -} - -template > -constexpr dim_t dim(std::ptrdiff_t n) noexcept -{ - return dim_t<>(n); -} - -template -class multi_span; -template -class strided_span; - -namespace details -{ - template - struct SpanTypeTraits - { - using value_type = T; - using size_type = std::size_t; - }; - - template - struct SpanTypeTraits::type> - { - using value_type = typename Traits::span_traits::value_type; - using size_type = typename Traits::span_traits::size_type; - }; - - template - struct SpanArrayTraits - { - using type = multi_span; - using value_type = T; - using bounds_type = static_bounds; - using pointer = T*; - using reference = T&; - }; - template - struct SpanArrayTraits : SpanArrayTraits - { - }; - - template - BoundsType newBoundsHelperImpl(std::ptrdiff_t totalSize, std::true_type) // dynamic size - { - Expects(totalSize >= 0 && totalSize <= PTRDIFF_MAX); - return BoundsType{totalSize}; - } - template - BoundsType newBoundsHelperImpl(std::ptrdiff_t totalSize, std::false_type) // static size - { - Expects(BoundsType::static_size <= totalSize); - return {}; - } - template - BoundsType newBoundsHelper(std::ptrdiff_t totalSize) - { - static_assert(BoundsType::dynamic_rank <= 1, "dynamic rank must less or equal to 1"); - return newBoundsHelperImpl( - totalSize, std::integral_constant()); - } - - struct Sep - { - }; - - template - T static_as_multi_span_helper(Sep, Args... args) - { - return T{narrow_cast(args)...}; - } - template - std::enable_if_t< - !std::is_same>::value && !std::is_same::value, T> - static_as_multi_span_helper(Arg, Args... args) - { - return static_as_multi_span_helper(args...); - } - template - T static_as_multi_span_helper(dim_t val, Args... args) - { - return static_as_multi_span_helper(args..., val.dvalue); - } - - template - struct static_as_multi_span_static_bounds_helper - { - using type = static_bounds<(Dimensions::value)...>; - }; - - template - struct is_multi_span_oracle : std::false_type - { - }; - - template - struct is_multi_span_oracle> - : std::true_type - { - }; - - template - struct is_multi_span_oracle> : std::true_type - { - }; - - template - struct is_multi_span : is_multi_span_oracle> - { - }; -} // namespace details - -template -class multi_span -{ - // TODO do we still need this? - template - friend class multi_span; - -public: - using bounds_type = static_bounds; - static const std::size_t Rank = bounds_type::rank; - using size_type = typename bounds_type::size_type; - using index_type = typename bounds_type::index_type; - using value_type = ValueType; - using const_value_type = std::add_const_t; - using pointer = std::add_pointer_t; - using reference = std::add_lvalue_reference_t; - using iterator = contiguous_span_iterator; - using const_span = multi_span; - using const_iterator = contiguous_span_iterator; - using reverse_iterator = std::reverse_iterator; - using const_reverse_iterator = std::reverse_iterator; - using sliced_type = - std::conditional_t>; - -private: - pointer data_; - bounds_type bounds_; - - friend iterator; - friend const_iterator; - -public: - // default constructor - same as constructing from nullptr_t - GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive - constexpr multi_span() noexcept - : multi_span(nullptr, bounds_type{}) - { - static_assert(bounds_type::dynamic_rank != 0 || - (bounds_type::dynamic_rank == 0 && bounds_type::static_size == 0), - "Default construction of multi_span only possible " - "for dynamic or fixed, zero-length spans."); - } - - // construct from nullptr - get an empty multi_span - GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive - constexpr multi_span(std::nullptr_t) noexcept - : multi_span(nullptr, bounds_type{}) - { - static_assert(bounds_type::dynamic_rank != 0 || - (bounds_type::dynamic_rank == 0 && bounds_type::static_size == 0), - "nullptr_t construction of multi_span only possible " - "for dynamic or fixed, zero-length spans."); - } - - // construct from nullptr with size of 0 (helps with template function calls) - template ::value>> - - // GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive // TODO: parser bug - constexpr multi_span(std::nullptr_t, IntType size) : multi_span(nullptr, bounds_type{}) - { - static_assert(bounds_type::dynamic_rank != 0 || - (bounds_type::dynamic_rank == 0 && bounds_type::static_size == 0), - "nullptr_t construction of multi_span only possible " - "for dynamic or fixed, zero-length spans."); - Expects(size == 0); - } - - // construct from a single element - - GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive - constexpr multi_span(reference data) noexcept - : multi_span(&data, bounds_type{1}) - { - static_assert(bounds_type::dynamic_rank > 0 || bounds_type::static_size == 0 || - bounds_type::static_size == 1, - "Construction from a single element only possible " - "for dynamic or fixed spans of length 0 or 1."); - } - - // prevent constructing from temporaries for single-elements - constexpr multi_span(value_type&&) = delete; - - // construct from pointer + length - GSL_SUPPRESS(type.6) // NO-FORMAT: attribute // TODO: false positive - constexpr multi_span(pointer ptr, size_type size) - : multi_span(ptr, bounds_type{size}) - {} - - // construct from pointer + length - multidimensional - constexpr multi_span(pointer data, bounds_type bounds) - : data_(data), bounds_(std::move(bounds)) - { - Expects((bounds_.size() > 0 && data != nullptr) || bounds_.size() == 0); - } - - // construct from begin,end pointer pair - template ::value && - details::LessThan::value>> - constexpr multi_span(pointer begin, Ptr end) - : multi_span(begin, - details::newBoundsHelper(static_cast(end) - begin)) - { - Expects(begin != nullptr && end != nullptr && begin <= static_cast(end)); - } - - // construct from n-dimensions static array - template > - constexpr multi_span(T (&arr)[N]) - : multi_span(reinterpret_cast(arr), bounds_type{typename Helper::bounds_type{}}) - { - static_assert(std::is_convertible::value, - "Cannot convert from source type to target multi_span type."); - static_assert(std::is_convertible::value, - "Cannot construct a multi_span from an array with fewer elements."); - } - - // construct from n-dimensions dynamic array (e.g. new int[m][4]) - // (precedence will be lower than the 1-dimension pointer) - template > - constexpr multi_span(T* const& data, size_type size) - : multi_span(reinterpret_cast(data), typename Helper::bounds_type{size}) - { - static_assert(std::is_convertible::value, - "Cannot convert from source type to target multi_span type."); - } - - // construct from std::array - template - constexpr multi_span(std::array& arr) - : multi_span(arr.data(), bounds_type{static_bounds{}}) - { - static_assert( - std::is_convertible(*)[]>::value, - "Cannot convert from source type to target multi_span type."); - static_assert(std::is_convertible, bounds_type>::value, - "You cannot construct a multi_span from a std::array of smaller size."); - } - - // construct from const std::array - template - constexpr multi_span(const std::array& arr) - : multi_span(arr.data(), bounds_type{static_bounds{}}) - { - static_assert( - std::is_convertible(*)[]>::value, - "Cannot convert from source type to target multi_span type."); - static_assert(std::is_convertible, bounds_type>::value, - "You cannot construct a multi_span from a std::array of smaller size."); - } - - // prevent constructing from temporary std::array - template - constexpr multi_span(std::array&& arr) = delete; - - // construct from containers - // future: could use contiguous_iterator_traits to identify only contiguous containers - // type-requirements: container must have .size(), operator[] which are value_type compatible - template ::value && - std::is_convertible::value && - std::is_same().size(), - *std::declval().data())>, - DataType>::value>> - constexpr multi_span(Cont& cont) - : multi_span(static_cast(cont.data()), - details::newBoundsHelper(narrow_cast(cont.size()))) - {} - - // prevent constructing from temporary containers - template ::value && - std::is_convertible::value && - std::is_same().size(), - *std::declval().data())>, - DataType>::value>> - explicit constexpr multi_span(Cont&& cont) = delete; - - // construct from a convertible multi_span - template , - typename = std::enable_if_t::value && - std::is_convertible::value>> - constexpr multi_span(multi_span other) - : data_(other.data_), bounds_(other.bounds_) - {} - - // trivial copy and move - constexpr multi_span(const multi_span&) = default; - constexpr multi_span(multi_span&&) = default; - - // trivial assignment - constexpr multi_span& operator=(const multi_span&) = default; - constexpr multi_span& operator=(multi_span&&) = default; - - // first() - extract the first Count elements into a new multi_span - template - - constexpr multi_span first() const - { - static_assert(Count >= 0, "Count must be >= 0."); - static_assert(bounds_type::static_size == dynamic_range || - Count <= bounds_type::static_size, - "Count is out of bounds."); - - Expects(bounds_type::static_size != dynamic_range || Count <= this->size()); - return {this->data(), Count}; - } - - // first() - extract the first count elements into a new multi_span - constexpr multi_span first(size_type count) const - { - Expects(count >= 0 && count <= this->size()); - return {this->data(), count}; - } - - // last() - extract the last Count elements into a new multi_span - template - constexpr multi_span last() const - { - static_assert(Count >= 0, "Count must be >= 0."); - static_assert(bounds_type::static_size == dynamic_range || - Count <= bounds_type::static_size, - "Count is out of bounds."); - - Expects(bounds_type::static_size != dynamic_range || Count <= this->size()); - return {this->data() + this->size() - Count, Count}; - } - - // last() - extract the last count elements into a new multi_span - constexpr multi_span last(size_type count) const - { - Expects(count >= 0 && count <= this->size()); - return {this->data() + this->size() - count, count}; - } - - // subspan() - create a subview of Count elements starting at Offset - template - constexpr multi_span subspan() const - { - static_assert(Count >= 0, "Count must be >= 0."); - static_assert(Offset >= 0, "Offset must be >= 0."); - static_assert(bounds_type::static_size == dynamic_range || - ((Offset <= bounds_type::static_size) && - Count <= bounds_type::static_size - Offset), - "You must describe a sub-range within bounds of the multi_span."); - - Expects(bounds_type::static_size != dynamic_range || - (Offset <= this->size() && Count <= this->size() - Offset)); - return {this->data() + Offset, Count}; - } - - // subspan() - create a subview of count elements starting at offset - // supplying dynamic_range for count will consume all available elements from offset - constexpr multi_span subspan(size_type offset, - size_type count = dynamic_range) const - { - Expects((offset >= 0 && offset <= this->size()) && - (count == dynamic_range || (count <= this->size() - offset))); - return {this->data() + offset, count == dynamic_range ? this->length() - offset : count}; - } - - // section - creates a non-contiguous, strided multi_span from a contiguous one - constexpr strided_span section(index_type origin, - index_type extents) const - { - const size_type size = this->bounds().total_size() - this->bounds().linearize(origin); - return {&this->operator[](origin), size, - strided_bounds{extents, details::make_stride(bounds())}}; - } - - // length of the multi_span in elements - constexpr size_type size() const noexcept { return bounds_.size(); } - - // length of the multi_span in elements - constexpr size_type length() const noexcept { return this->size(); } - - // length of the multi_span in bytes - constexpr size_type size_bytes() const noexcept - { - return narrow_cast(sizeof(value_type)) * this->size(); - } - - // length of the multi_span in bytes - constexpr size_type length_bytes() const noexcept { return this->size_bytes(); } - - constexpr bool empty() const noexcept { return this->size() == 0; } - - static constexpr std::size_t rank() { return Rank; } - - template - constexpr size_type extent() const noexcept - { - static_assert(Dim < Rank, - "Dimension should be less than rank (dimension count starts from 0)."); - return bounds_.template extent(); - } - - template - constexpr size_type extent(IntType dim) const - { - return bounds_.extent(dim); - } - - constexpr bounds_type bounds() const noexcept { return bounds_; } - - constexpr pointer data() const noexcept { return data_; } - - template - constexpr reference operator()(FirstIndex idx) - { - return this->operator[](narrow_cast(idx)); - } - - template - constexpr reference operator()(FirstIndex firstIndex, OtherIndices... indices) - { - const index_type idx = {narrow_cast(firstIndex), - narrow_cast(indices)...}; - return this->operator[](idx); - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr reference operator[](const index_type& idx) const - { - return data_[bounds_.linearize(idx)]; - } - - template 1), typename Ret = std::enable_if_t> - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr Ret operator[](size_type idx) const - { - Expects(idx >= 0 && idx < bounds_.size()); // index is out of bounds of the array - const size_type ridx = idx * bounds_.stride(); - - // index is out of bounds of the underlying data - Expects(ridx < bounds_.total_size()); - return Ret{data_ + ridx, bounds_.slice()}; - } - - constexpr iterator begin() const noexcept { return iterator{this, true}; } - - constexpr iterator end() const noexcept { return iterator{this, false}; } - - GSL_SUPPRESS(type.1) // NO-FORMAT: attribute - constexpr const_iterator cbegin() const noexcept - { - return const_iterator{reinterpret_cast(this), true}; - } - - constexpr const_iterator cend() const noexcept - { - return const_iterator{reinterpret_cast(this), false}; - } - - constexpr reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; } - - constexpr reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; } - - constexpr const_reverse_iterator crbegin() const noexcept - { - return const_reverse_iterator{cend()}; - } - - constexpr const_reverse_iterator crend() const noexcept - { - return const_reverse_iterator{cbegin()}; - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator==(const multi_span& other) const - { - return bounds_.size() == other.bounds_.size() && - (data_ == other.data_ || std::equal(this->begin(), this->end(), other.begin())); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator!=(const multi_span& other) const - { - return !(*this == other); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator<(const multi_span& other) const - { - return std::lexicographical_compare(this->begin(), this->end(), other.begin(), other.end()); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator<=(const multi_span& other) const - { - return !(other < *this); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator>(const multi_span& other) const - noexcept - { - return (other < *this); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator>=(const multi_span& other) const - { - return !(*this < other); - } -}; - -// -// Free functions for manipulating spans -// - -// reshape a multi_span into a different dimensionality -// DimCount and Enabled here are workarounds for a bug in MSVC 2015 -template 0), typename = std::enable_if_t> -constexpr auto as_multi_span(SpanType s, Dimensions2... dims) - -> multi_span -{ - static_assert(details::is_multi_span::value, - "Variadic as_multi_span() is for reshaping existing spans."); - using BoundsType = - typename multi_span::bounds_type; - const auto tobounds = details::static_as_multi_span_helper(dims..., details::Sep{}); - details::verifyBoundsReshape(s.bounds(), tobounds); - return {s.data(), tobounds}; -} - -// convert a multi_span to a multi_span -template -multi_span as_bytes(multi_span s) noexcept -{ - static_assert(std::is_trivial>::value, - "The value_type of multi_span must be a trivial type."); - return {reinterpret_cast(s.data()), s.size_bytes()}; -} - -// convert a multi_span to a multi_span (a writeable byte multi_span) -// this is not currently a portable function that can be relied upon to work -// on all implementations. It should be considered an experimental extension -// to the standard GSL interface. -template -multi_span as_writeable_bytes(multi_span s) noexcept -{ - static_assert(std::is_trivial>::value, - "The value_type of multi_span must be a trivial type."); - return {reinterpret_cast(s.data()), s.size_bytes()}; -} - -// convert a multi_span to a multi_span -// this is not currently a portable function that can be relied upon to work -// on all implementations. It should be considered an experimental extension -// to the standard GSL interface. -template -constexpr auto as_multi_span(multi_span s) -> multi_span< - const U, static_cast( - multi_span::bounds_type::static_size != dynamic_range - ? (static_cast( - multi_span::bounds_type::static_size) / - sizeof(U)) - : dynamic_range)> -{ - using ConstByteSpan = multi_span; - static_assert( - std::is_trivial>::value && - (ConstByteSpan::bounds_type::static_size == dynamic_range || - ConstByteSpan::bounds_type::static_size % narrow_cast(sizeof(U)) == 0), - "Target type must be a trivial type and its size must match the byte array size"); - - Expects((s.size_bytes() % narrow_cast(sizeof(U))) == 0 && - (s.size_bytes() / narrow_cast(sizeof(U))) < PTRDIFF_MAX); - return {reinterpret_cast(s.data()), - s.size_bytes() / narrow_cast(sizeof(U))}; -} - -// convert a multi_span to a multi_span -// this is not currently a portable function that can be relied upon to work -// on all implementations. It should be considered an experimental extension -// to the standard GSL interface. -template -constexpr auto as_multi_span(multi_span s) - -> multi_span( - multi_span::bounds_type::static_size != dynamic_range - ? static_cast( - multi_span::bounds_type::static_size) / - sizeof(U) - : dynamic_range)> -{ - using ByteSpan = multi_span; - static_assert(std::is_trivial>::value && - (ByteSpan::bounds_type::static_size == dynamic_range || - ByteSpan::bounds_type::static_size % sizeof(U) == 0), - "Target type must be a trivial type and its size must match the byte array size"); - - Expects((s.size_bytes() % sizeof(U)) == 0); - return {reinterpret_cast(s.data()), - s.size_bytes() / narrow_cast(sizeof(U))}; -} - -template -constexpr auto as_multi_span(T* const& ptr, dim_t... args) - -> multi_span, Dimensions...> -{ - return {reinterpret_cast*>(ptr), - details::static_as_multi_span_helper>(args..., - details::Sep{})}; -} - -template -constexpr auto as_multi_span(T* arr, std::ptrdiff_t len) -> - typename details::SpanArrayTraits::type -{ - return {reinterpret_cast*>(arr), len}; -} - -template -constexpr auto as_multi_span(T (&arr)[N]) -> typename details::SpanArrayTraits::type -{ - return {arr}; -} - -template -constexpr multi_span as_multi_span(const std::array& arr) -{ - return {arr}; -} - -template -constexpr multi_span as_multi_span(const std::array&&) = delete; - -template -constexpr multi_span as_multi_span(std::array& arr) -{ - return {arr}; -} - -template -constexpr multi_span as_multi_span(T* begin, T* end) -{ - return {begin, end}; -} - -template -constexpr auto as_multi_span(Cont& arr) -> std::enable_if_t< - !details::is_multi_span>::value, - multi_span, dynamic_range>> -{ - Expects(arr.size() < PTRDIFF_MAX); - return {arr.data(), narrow_cast(arr.size())}; -} - -template -constexpr auto as_multi_span(Cont&& arr) -> std::enable_if_t< - !details::is_multi_span>::value, - multi_span, dynamic_range>> = delete; - -// from basic_string which doesn't have nonconst .data() member like other contiguous containers -template -GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute -constexpr auto as_multi_span(std::basic_string& str) - -> multi_span -{ - Expects(str.size() < PTRDIFF_MAX); - return {&str[0], narrow_cast(str.size())}; -} - -// strided_span is an extension that is not strictly part of the GSL at this time. -// It is kept here while the multidimensional interface is still being defined. -template -class strided_span -{ -public: - using bounds_type = strided_bounds; - using size_type = typename bounds_type::size_type; - using index_type = typename bounds_type::index_type; - using value_type = ValueType; - using const_value_type = std::add_const_t; - using pointer = std::add_pointer_t; - using reference = std::add_lvalue_reference_t; - using iterator = general_span_iterator; - using const_strided_span = strided_span; - using const_iterator = general_span_iterator; - using reverse_iterator = std::reverse_iterator; - using const_reverse_iterator = std::reverse_iterator; - using sliced_type = - std::conditional_t>; - -private: - pointer data_; - bounds_type bounds_; - - friend iterator; - friend const_iterator; - template - friend class strided_span; - -public: - // from raw data - constexpr strided_span(pointer ptr, size_type size, bounds_type bounds) - : data_(ptr), bounds_(std::move(bounds)) - { - Expects((bounds_.size() > 0 && ptr != nullptr) || bounds_.size() == 0); - // Bounds cross data boundaries - Expects(this->bounds().total_size() <= size); - GSL_SUPPRESS(type.4) // NO-FORMAT: attribute // TODO: false positive - (void) size; - } - - // from static array of size N - template - constexpr strided_span(value_type (&values)[N], bounds_type bounds) - : strided_span(values, N, std::move(bounds)) - {} - - // from array view - template ::value, - typename = std::enable_if_t> - constexpr strided_span(multi_span av, bounds_type bounds) - : strided_span(av.data(), av.bounds().total_size(), std::move(bounds)) - {} - - // convertible - template ::value>> - constexpr strided_span(const strided_span& other) - : data_(other.data_), bounds_(other.bounds_) - {} - - // convert from bytes - template - constexpr strided_span< - typename std::enable_if::value, OtherValueType>::type, - Rank> - as_strided_span() const - { - static_assert((sizeof(OtherValueType) >= sizeof(value_type)) && - (sizeof(OtherValueType) % sizeof(value_type) == 0), - "OtherValueType should have a size to contain a multiple of ValueTypes"); - auto d = narrow_cast(sizeof(OtherValueType) / sizeof(value_type)); - - const size_type size = this->bounds().total_size() / d; - - GSL_SUPPRESS(type.3) // NO-FORMAT: attribute - return {const_cast(reinterpret_cast(this->data())), - size, - bounds_type{resize_extent(this->bounds().index_bounds(), d), - resize_stride(this->bounds().strides(), d)}}; - } - - constexpr strided_span section(index_type origin, index_type extents) const - { - const size_type size = this->bounds().total_size() - this->bounds().linearize(origin); - return {&this->operator[](origin), size, - bounds_type{extents, details::make_stride(bounds())}}; - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr reference operator[](const index_type& idx) const - { - return data_[bounds_.linearize(idx)]; - } - - template 1), typename Ret = std::enable_if_t> - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr Ret operator[](size_type idx) const - { - Expects(idx < bounds_.size()); // index is out of bounds of the array - const size_type ridx = idx * bounds_.stride(); - - // index is out of bounds of the underlying data - Expects(ridx < bounds_.total_size()); - return {data_ + ridx, bounds_.slice().total_size(), bounds_.slice()}; - } - - constexpr bounds_type bounds() const noexcept { return bounds_; } - - template - constexpr size_type extent() const noexcept - { - static_assert(Dim < Rank, - "dimension should be less than Rank (dimension count starts from 0)"); - return bounds_.template extent(); - } - - constexpr size_type size() const noexcept { return bounds_.size(); } - - constexpr pointer data() const noexcept { return data_; } - - constexpr bool empty() const noexcept { return this->size() == 0; } - - constexpr explicit operator bool() const noexcept { return data_ != nullptr; } - - constexpr iterator begin() const { return iterator{this, true}; } - - constexpr iterator end() const { return iterator{this, false}; } - - constexpr const_iterator cbegin() const - { - return const_iterator{reinterpret_cast(this), true}; - } - - constexpr const_iterator cend() const - { - return const_iterator{reinterpret_cast(this), false}; - } - - constexpr reverse_iterator rbegin() const { return reverse_iterator{end()}; } - - constexpr reverse_iterator rend() const { return reverse_iterator{begin()}; } - - constexpr const_reverse_iterator crbegin() const { return const_reverse_iterator{cend()}; } - - constexpr const_reverse_iterator crend() const { return const_reverse_iterator{cbegin()}; } - - template , - std::remove_cv_t>::value>> - constexpr bool operator==(const strided_span& other) const - { - return bounds_.size() == other.bounds_.size() && - (data_ == other.data_ || std::equal(this->begin(), this->end(), other.begin())); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator!=(const strided_span& other) const - { - return !(*this == other); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator<(const strided_span& other) const - { - return std::lexicographical_compare(this->begin(), this->end(), other.begin(), other.end()); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator<=(const strided_span& other) const - { - return !(other < *this); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator>(const strided_span& other) const - { - return (other < *this); - } - - template , - std::remove_cv_t>::value>> - constexpr bool operator>=(const strided_span& other) const - { - return !(*this < other); - } - -private: - static index_type resize_extent(const index_type& extent, std::ptrdiff_t d) - { - // The last dimension of the array needs to contain a multiple of new type elements - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - Expects(extent[Rank - 1] >= d && (extent[Rank - 1] % d == 0)); - - index_type ret = extent; - ret[Rank - 1] /= d; - - return ret; - } - - template > - static index_type resize_stride(const index_type& strides, std::ptrdiff_t, void* = nullptr) - { - // Only strided arrays with regular strides can be resized - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - Expects(strides[Rank - 1] == 1); - - return strides; - } - - template 1), typename = std::enable_if_t> - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - static index_type resize_stride(const index_type& strides, std::ptrdiff_t d) - { - // Only strided arrays with regular strides can be resized - Expects(strides[Rank - 1] == 1); - // The strides must have contiguous chunks of - // memory that can contain a multiple of new type elements - Expects(strides[Rank - 2] >= d && (strides[Rank - 2] % d == 0)); - - for (std::size_t i = Rank - 1; i > 0; --i) - { - // Only strided arrays with regular strides can be resized - Expects((strides[i - 1] >= strides[i]) && (strides[i - 1] % strides[i] == 0)); - } - - index_type ret = strides / d; - ret[Rank - 1] = 1; - - return ret; - } -}; - -template -class contiguous_span_iterator -{ -public: - using iterator_category = std::random_access_iterator_tag; - using value_type = typename Span::value_type; - using difference_type = std::ptrdiff_t; - using pointer = value_type*; - using reference = value_type&; - -private: - template - friend class multi_span; - - pointer data_; - const Span* m_validator; - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - void validateThis() const { - // iterator is out of range of the array - Expects(data_ >= m_validator->data_ && data_ < m_validator->data_ + m_validator->size()); - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - contiguous_span_iterator(const Span* container, bool isbegin) - : data_(isbegin ? container->data_ : container->data_ + container->size()) - , m_validator(container) - {} - -public: - reference operator*() const - { - validateThis(); - return *data_; - } - pointer operator->() const - { - validateThis(); - return data_; - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - contiguous_span_iterator& operator++() noexcept - { - ++data_; - return *this; - } - contiguous_span_iterator operator++(int) noexcept - { - auto ret = *this; - ++(*this); - return ret; - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - contiguous_span_iterator& operator--() noexcept - { - --data_; - return *this; - } - contiguous_span_iterator operator--(int) noexcept - { - auto ret = *this; - --(*this); - return ret; - } - contiguous_span_iterator operator+(difference_type n) const noexcept - { - contiguous_span_iterator ret{*this}; - return ret += n; - } - contiguous_span_iterator& operator+=(difference_type n) noexcept - { - data_ += n; - return *this; - } - contiguous_span_iterator operator-(difference_type n) const noexcept - { - contiguous_span_iterator ret{*this}; - return ret -= n; - } - - contiguous_span_iterator& operator-=(difference_type n) { return *this += -n; } - difference_type operator-(const contiguous_span_iterator& rhs) const - { - Expects(m_validator == rhs.m_validator); - return data_ - rhs.data_; - } - reference operator[](difference_type n) const { return *(*this + n); } - bool operator==(const contiguous_span_iterator& rhs) const - { - Expects(m_validator == rhs.m_validator); - return data_ == rhs.data_; - } - - bool operator!=(const contiguous_span_iterator& rhs) const { return !(*this == rhs); } - - bool operator<(const contiguous_span_iterator& rhs) const - { - Expects(m_validator == rhs.m_validator); - return data_ < rhs.data_; - } - - bool operator<=(const contiguous_span_iterator& rhs) const { return !(rhs < *this); } - bool operator>(const contiguous_span_iterator& rhs) const { return rhs < *this; } - bool operator>=(const contiguous_span_iterator& rhs) const { return !(rhs > *this); } - - void swap(contiguous_span_iterator& rhs) noexcept - { - std::swap(data_, rhs.data_); - std::swap(m_validator, rhs.m_validator); - } -}; - -template -contiguous_span_iterator operator+(typename contiguous_span_iterator::difference_type n, - const contiguous_span_iterator& rhs) noexcept -{ - return rhs + n; -} - -template -class general_span_iterator { -public: - using iterator_category = std::random_access_iterator_tag; - using value_type = typename Span::value_type; - using difference_type = std::ptrdiff_t; - using pointer = value_type*; - using reference = value_type&; - -private: - template - friend class strided_span; - - const Span* m_container; - typename Span::bounds_type::iterator m_itr; - general_span_iterator(const Span* container, bool isbegin) - : m_container(container) - , m_itr(isbegin ? m_container->bounds().begin() : m_container->bounds().end()) - {} - -public: - reference operator*() noexcept { return (*m_container)[*m_itr]; } - pointer operator->() noexcept { return &(*m_container)[*m_itr]; } - general_span_iterator& operator++() noexcept - { - ++m_itr; - return *this; - } - general_span_iterator operator++(int) noexcept - { - auto ret = *this; - ++(*this); - return ret; - } - general_span_iterator& operator--() noexcept - { - --m_itr; - return *this; - } - general_span_iterator operator--(int) noexcept - { - auto ret = *this; - --(*this); - return ret; - } - general_span_iterator operator+(difference_type n) const noexcept - { - general_span_iterator ret{*this}; - return ret += n; - } - general_span_iterator& operator+=(difference_type n) noexcept - { - m_itr += n; - return *this; - } - general_span_iterator operator-(difference_type n) const noexcept - { - general_span_iterator ret{*this}; - return ret -= n; - } - general_span_iterator& operator-=(difference_type n) noexcept { return *this += -n; } - difference_type operator-(const general_span_iterator& rhs) const - { - Expects(m_container == rhs.m_container); - return m_itr - rhs.m_itr; - } - - GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute - value_type operator[](difference_type n) const { return (*m_container)[m_itr[n]]; } - - bool operator==(const general_span_iterator& rhs) const - { - Expects(m_container == rhs.m_container); - return m_itr == rhs.m_itr; - } - bool operator!=(const general_span_iterator& rhs) const { return !(*this == rhs); } - bool operator<(const general_span_iterator& rhs) const - { - Expects(m_container == rhs.m_container); - return m_itr < rhs.m_itr; - } - bool operator<=(const general_span_iterator& rhs) const { return !(rhs < *this); } - bool operator>(const general_span_iterator& rhs) const { return rhs < *this; } - bool operator>=(const general_span_iterator& rhs) const { return !(rhs > *this); } - void swap(general_span_iterator& rhs) noexcept - { - std::swap(m_itr, rhs.m_itr); - std::swap(m_container, rhs.m_container); - } -}; - -template -general_span_iterator operator+(typename general_span_iterator::difference_type n, - const general_span_iterator& rhs) noexcept -{ - return rhs + n; -} - -} // namespace gsl - -#if defined(_MSC_VER) && !defined(__clang__) -#if _MSC_VER < 1910 - -#undef constexpr -#pragma pop_macro("constexpr") -#endif // _MSC_VER < 1910 - -#pragma warning(pop) - -#endif // _MSC_VER - -#if defined(__GNUC__) && __GNUC__ > 6 -#pragma GCC diagnostic pop -#endif // __GNUC__ > 6 - -#endif // GSL_MULTI_SPAN_H diff --git a/src/algorithms/libs/gsl/include/gsl/pointers b/src/algorithms/libs/gsl/include/gsl/pointers deleted file mode 100644 index 0f2987a0a..000000000 --- a/src/algorithms/libs/gsl/include/gsl/pointers +++ /dev/null @@ -1,294 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_POINTERS_H -#define GSL_POINTERS_H - -#include // for Ensures, Expects - -#include // for forward -#include // for ptrdiff_t, nullptr_t, ostream, size_t -#include // for shared_ptr, unique_ptr -#include // for hash -#include // for enable_if_t, is_convertible, is_assignable - -#if defined(_MSC_VER) && _MSC_VER < 1910 && !defined(__clang__) -#pragma push_macro("constexpr") -#define constexpr /*constexpr*/ - -#endif // defined(_MSC_VER) && _MSC_VER < 1910 - -namespace gsl -{ - -// -// GSL.owner: ownership pointers -// -using std::unique_ptr; -using std::shared_ptr; - -// -// owner -// -// owner is designed as a bridge for code that must deal directly with owning pointers for some reason -// -// T must be a pointer type -// - disallow construction from any type other than pointer type -// -template ::value>> -using owner = T; - -// -// not_null -// -// Restricts a pointer or smart pointer to only hold non-null values. -// -// Has zero size overhead over T. -// -// If T is a pointer (i.e. T == U*) then -// - allow construction from U* -// - disallow construction from nullptr_t -// - disallow default construction -// - ensure construction from null U* fails -// - allow implicit conversion to U* -// -template -class not_null -{ -public: - static_assert(std::is_assignable::value, "T cannot be assigned nullptr."); - - template ::value>> - constexpr not_null(U&& u) : ptr_(std::forward(u)) - { - Expects(ptr_ != nullptr); - } - - template ::value>> - constexpr not_null(T u) : ptr_(u) - { - Expects(ptr_ != nullptr); - } - - template ::value>> - constexpr not_null(const not_null& other) : not_null(other.get()) - { - } - - not_null(not_null&& other) = default; - not_null(const not_null& other) = default; - not_null& operator=(const not_null& other) = default; - - constexpr T get() const - { - Ensures(ptr_ != nullptr); - return ptr_; - } - - constexpr operator T() const { return get(); } - constexpr T operator->() const { return get(); } - constexpr decltype(auto) operator*() const { return *get(); } - - // prevents compilation when someone attempts to assign a null pointer constant - not_null(std::nullptr_t) = delete; - not_null& operator=(std::nullptr_t) = delete; - - // unwanted operators...pointers only point to single objects! - not_null& operator++() = delete; - not_null& operator--() = delete; - not_null operator++(int) = delete; - not_null operator--(int) = delete; - not_null& operator+=(std::ptrdiff_t) = delete; - not_null& operator-=(std::ptrdiff_t) = delete; - void operator[](std::ptrdiff_t) const = delete; - -private: - T ptr_; -}; - -template -auto make_not_null(T&& t) { - return not_null>>{std::forward(t)}; -} - -template -std::ostream& operator<<(std::ostream& os, const not_null& val) -{ - os << val.get(); - return os; -} - -template -auto operator==(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() == rhs.get()) -{ - return lhs.get() == rhs.get(); -} - -template -auto operator!=(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() != rhs.get()) -{ - return lhs.get() != rhs.get(); -} - -template -auto operator<(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() < rhs.get()) -{ - return lhs.get() < rhs.get(); -} - -template -auto operator<=(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() <= rhs.get()) -{ - return lhs.get() <= rhs.get(); -} - -template -auto operator>(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() > rhs.get()) -{ - return lhs.get() > rhs.get(); -} - -template -auto operator>=(const not_null& lhs, const not_null& rhs) -> decltype(lhs.get() >= rhs.get()) -{ - return lhs.get() >= rhs.get(); -} - -// more unwanted operators -template -std::ptrdiff_t operator-(const not_null&, const not_null&) = delete; -template -not_null operator-(const not_null&, std::ptrdiff_t) = delete; -template -not_null operator+(const not_null&, std::ptrdiff_t) = delete; -template -not_null operator+(std::ptrdiff_t, const not_null&) = delete; - -} // namespace gsl - -namespace std -{ -template -struct hash> -{ - std::size_t operator()(const gsl::not_null& value) const { return hash{}(value); } -}; - -} // namespace std - -namespace gsl -{ - -// -// strict_not_null -// -// Restricts a pointer or smart pointer to only hold non-null values, -// -// - provides a strict (i.e. explicit contructor from T) wrapper of not_null -// - to be used for new code that wishes the design to be cleaner and make not_null -// checks intentional, or in old code that would like to make the transition. -// -// To make the transition from not_null, incrementally replace not_null -// by strict_not_null and fix compilation errors -// -// Expect to -// - remove all unneded conversions from raw pointer to not_null and back -// - make API clear by specifyning not_null in parameters where needed -// - remove unnesessary asserts -// -template -class strict_not_null: public not_null -{ -public: - - template ::value>> - constexpr explicit strict_not_null(U&& u) : - not_null(std::forward(u)) - {} - - template ::value>> - constexpr explicit strict_not_null(T u) : - not_null(u) - {} - - template ::value>> - constexpr strict_not_null(const not_null& other) : - not_null(other) - {} - - template ::value>> - constexpr strict_not_null(const strict_not_null& other) : - not_null(other) - {} - - strict_not_null(strict_not_null&& other) = default; - strict_not_null(const strict_not_null& other) = default; - strict_not_null& operator=(const strict_not_null& other) = default; - strict_not_null& operator=(const not_null& other) - { - not_null::operator=(other); - return *this; - } - - // prevents compilation when someone attempts to assign a null pointer constant - strict_not_null(std::nullptr_t) = delete; - strict_not_null& operator=(std::nullptr_t) = delete; - - // unwanted operators...pointers only point to single objects! - strict_not_null& operator++() = delete; - strict_not_null& operator--() = delete; - strict_not_null operator++(int) = delete; - strict_not_null operator--(int) = delete; - strict_not_null& operator+=(std::ptrdiff_t) = delete; - strict_not_null& operator-=(std::ptrdiff_t) = delete; - void operator[](std::ptrdiff_t) const = delete; -}; - -// more unwanted operators -template -std::ptrdiff_t operator-(const strict_not_null&, const strict_not_null&) = delete; -template -strict_not_null operator-(const strict_not_null&, std::ptrdiff_t) = delete; -template -strict_not_null operator+(const strict_not_null&, std::ptrdiff_t) = delete; -template -strict_not_null operator+(std::ptrdiff_t, const strict_not_null&) = delete; - -template -auto make_strict_not_null(T&& t) { - return strict_not_null>>{std::forward(t)}; -} - -} // namespace gsl - -namespace std -{ -template -struct hash> -{ - std::size_t operator()(const gsl::strict_not_null& value) const { return hash{}(value); } -}; - -} // namespace std - -#if defined(_MSC_VER) && _MSC_VER < 1910 && !defined(__clang__) - -#undef constexpr -#pragma pop_macro("constexpr") - -#endif // defined(_MSC_VER) && _MSC_VER < 1910 && !defined(__clang__) - -#endif // GSL_POINTERS_H diff --git a/src/algorithms/libs/gsl/include/gsl/span b/src/algorithms/libs/gsl/include/gsl/span deleted file mode 100644 index b4da53216..000000000 --- a/src/algorithms/libs/gsl/include/gsl/span +++ /dev/null @@ -1,793 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_SPAN_H -#define GSL_SPAN_H - -#include // for Expects -#include // for byte -#include // for narrow_cast, narrow - -#include // for lexicographical_compare -#include // for array -#include // for ptrdiff_t, size_t, nullptr_t -#include // for reverse_iterator, distance, random_access_... -#include -#include -#include // for enable_if_t, declval, is_convertible, inte... -#include -#include // for std::addressof - -#if defined(_MSC_VER) && !defined(__clang__) -#pragma warning(push) - -// turn off some warnings that are noisy about our Expects statements -#pragma warning(disable : 4127) // conditional expression is constant -#pragma warning(disable : 4702) // unreachable code - -// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. -#pragma warning(disable : 26495) // uninitalized member when constructor calls constructor -#pragma warning(disable : 26446) // parser bug does not allow attributes on some templates - -#if _MSC_VER < 1910 -#pragma push_macro("constexpr") -#define constexpr /*constexpr*/ -#define GSL_USE_STATIC_CONSTEXPR_WORKAROUND - -#endif // _MSC_VER < 1910 -#endif // _MSC_VER - -// See if we have enough C++17 power to use a static constexpr data member -// without needing an out-of-line definition -#if !(defined(__cplusplus) && (__cplusplus >= 201703L)) -#define GSL_USE_STATIC_CONSTEXPR_WORKAROUND -#endif // !(defined(__cplusplus) && (__cplusplus >= 201703L)) - -// GCC 7 does not like the signed unsigned missmatch (size_t ptrdiff_t) -// While there is a conversion from signed to unsigned, it happens at -// compiletime, so the compiler wouldn't have to warn indiscriminently, but -// could check if the source value actually doesn't fit into the target type -// and only warn in those cases. -#if defined(__GNUC__) && __GNUC__ > 6 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wsign-conversion" -#endif - -namespace gsl -{ - -// [views.constants], constants -constexpr const std::ptrdiff_t dynamic_extent = -1; - -template -class span; - -// implementation details -namespace details -{ - template - struct is_span_oracle : std::false_type - { - }; - - template - struct is_span_oracle> : std::true_type - { - }; - - template - struct is_span : public is_span_oracle> - { - }; - - template - struct is_std_array_oracle : std::false_type - { - }; - - template - struct is_std_array_oracle> : std::true_type - { - }; - - template - struct is_std_array : public is_std_array_oracle> - { - }; - - template - struct is_allowed_extent_conversion - : public std::integral_constant - { - }; - - template - struct is_allowed_element_type_conversion - : public std::integral_constant::value> - { - }; - - template - class span_iterator - { - using element_type_ = typename Span::element_type; - - public: -#ifdef _MSC_VER - // Tell Microsoft standard library that span_iterators are checked. - using _Unchecked_type = typename Span::pointer; -#endif - - using iterator_category = std::random_access_iterator_tag; - using value_type = std::remove_cv_t; - using difference_type = typename Span::index_type; - - using reference = std::conditional_t&; - using pointer = std::add_pointer_t; - - span_iterator() = default; - - constexpr span_iterator(const Span* span, typename Span::index_type idx) noexcept - : span_(span), index_(idx) - {} - - friend span_iterator; - template * = nullptr> - constexpr span_iterator(const span_iterator& other) noexcept - : span_iterator(other.span_, other.index_) - {} - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr reference operator*() const - { - Expects(index_ != span_->size()); - return *(span_->data() + index_); - } - - constexpr pointer operator->() const - { - Expects(index_ != span_->size()); - return span_->data() + index_; - } - - constexpr span_iterator& operator++() - { - Expects(0 <= index_ && index_ != span_->size()); - ++index_; - return *this; - } - - constexpr span_iterator operator++(int) - { - auto ret = *this; - ++(*this); - return ret; - } - - constexpr span_iterator& operator--() - { - Expects(index_ != 0 && index_ <= span_->size()); - --index_; - return *this; - } - - constexpr span_iterator operator--(int) - { - auto ret = *this; - --(*this); - return ret; - } - - constexpr span_iterator operator+(difference_type n) const - { - auto ret = *this; - return ret += n; - } - - friend constexpr span_iterator operator+(difference_type n, span_iterator const& rhs) - { - return rhs + n; - } - - constexpr span_iterator& operator+=(difference_type n) - { - Expects((index_ + n) >= 0 && (index_ + n) <= span_->size()); - index_ += n; - return *this; - } - - constexpr span_iterator operator-(difference_type n) const - { - auto ret = *this; - return ret -= n; - } - - constexpr span_iterator& operator-=(difference_type n) { return *this += -n; } - - constexpr difference_type operator-(span_iterator rhs) const - { - Expects(span_ == rhs.span_); - return index_ - rhs.index_; - } - - constexpr reference operator[](difference_type n) const { return *(*this + n); } - - constexpr friend bool operator==(span_iterator lhs, span_iterator rhs) noexcept - { - return lhs.span_ == rhs.span_ && lhs.index_ == rhs.index_; - } - - constexpr friend bool operator!=(span_iterator lhs, span_iterator rhs) noexcept - { - return !(lhs == rhs); - } - - constexpr friend bool operator<(span_iterator lhs, span_iterator rhs) noexcept - { - return lhs.index_ < rhs.index_; - } - - constexpr friend bool operator<=(span_iterator lhs, span_iterator rhs) noexcept - { - return !(rhs < lhs); - } - - constexpr friend bool operator>(span_iterator lhs, span_iterator rhs) noexcept - { - return rhs < lhs; - } - - constexpr friend bool operator>=(span_iterator lhs, span_iterator rhs) noexcept - { - return !(rhs > lhs); - } - -#ifdef _MSC_VER - // MSVC++ iterator debugging support; allows STL algorithms in 15.8+ - // to unwrap span_iterator to a pointer type after a range check in STL - // algorithm calls - friend constexpr void _Verify_range(span_iterator lhs, span_iterator rhs) noexcept - { // test that [lhs, rhs) forms a valid range inside an STL algorithm - Expects(lhs.span_ == rhs.span_ // range spans have to match - && lhs.index_ <= rhs.index_); // range must not be transposed - } - - constexpr void _Verify_offset(const difference_type n) const noexcept - { // test that the iterator *this + n is a valid range in an STL - // algorithm call - Expects((index_ + n) >= 0 && (index_ + n) <= span_->size()); - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr pointer _Unwrapped() const noexcept - { // after seeking *this to a high water mark, or using one of the - // _Verify_xxx functions above, unwrap this span_iterator to a raw - // pointer - return span_->data() + index_; - } - - // Tell the STL that span_iterator should not be unwrapped if it can't - // validate in advance, even in release / optimized builds: -#if defined(GSL_USE_STATIC_CONSTEXPR_WORKAROUND) - static constexpr const bool _Unwrap_when_unverified = false; -#else - static constexpr bool _Unwrap_when_unverified = false; -#endif - GSL_SUPPRESS(con.3) // NO-FORMAT: attribute // TODO: false positive - constexpr void _Seek_to(const pointer p) noexcept - { // adjust the position of *this to previously verified location p - // after _Unwrapped - index_ = p - span_->data(); - } -#endif - - protected: - const Span* span_ = nullptr; - std::ptrdiff_t index_ = 0; - }; - - template - class extent_type - { - public: - using index_type = std::ptrdiff_t; - - static_assert(Ext >= 0, "A fixed-size span must be >= 0 in size."); - - constexpr extent_type() noexcept {} - - template - constexpr extent_type(extent_type ext) - { - static_assert(Other == Ext || Other == dynamic_extent, - "Mismatch between fixed-size extent and size of initializing data."); - Expects(ext.size() == Ext); - } - - constexpr extent_type(index_type size) { Expects(size == Ext); } - - constexpr index_type size() const noexcept { return Ext; } - }; - - template <> - class extent_type - { - public: - using index_type = std::ptrdiff_t; - - template - explicit constexpr extent_type(extent_type ext) : size_(ext.size()) - {} - - explicit constexpr extent_type(index_type size) : size_(size) { Expects(size >= 0); } - - constexpr index_type size() const noexcept { return size_; } - - private: - index_type size_; - }; - - template - struct calculate_subspan_type - { - using type = span; - }; -} // namespace details - -// [span], class template span -template -class span -{ -public: - // constants and types - using element_type = ElementType; - using value_type = std::remove_cv_t; - using index_type = std::ptrdiff_t; - using pointer = element_type*; - using reference = element_type&; - - using iterator = details::span_iterator, false>; - using const_iterator = details::span_iterator, true>; - using reverse_iterator = std::reverse_iterator; - using const_reverse_iterator = std::reverse_iterator; - - using size_type = index_type; - -#if defined(GSL_USE_STATIC_CONSTEXPR_WORKAROUND) - static constexpr const index_type extent{Extent}; -#else - static constexpr index_type extent{Extent}; -#endif - - // [span.cons], span constructors, copy, assignment, and destructor - template " SFINAE, - // since "std::enable_if_t" is ill-formed when Extent is greater than 0. - class = std::enable_if_t<(Dependent || Extent <= 0)>> - constexpr span() noexcept : storage_(nullptr, details::extent_type<0>()) - {} - - constexpr span(pointer ptr, index_type count) : storage_(ptr, count) {} - - constexpr span(pointer firstElem, pointer lastElem) - : storage_(firstElem, std::distance(firstElem, lastElem)) - {} - - template - constexpr span(element_type (&arr)[N]) noexcept - : storage_(KnownNotNull{std::addressof(arr[0])}, details::extent_type()) - {} - - template 0)>> - constexpr span(std::array, N>& arr) noexcept - : storage_(KnownNotNull{arr.data()}, details::extent_type()) - { - } - - constexpr span(std::array, 0>&) noexcept - : storage_(static_cast(nullptr), details::extent_type<0>()) - { - } - - template 0)>> - constexpr span(const std::array, N>& arr) noexcept - : storage_(KnownNotNull{arr.data()}, details::extent_type()) - { - } - - constexpr span(const std::array, 0>&) noexcept - : storage_(static_cast(nullptr), details::extent_type<0>()) - { - } - - // NB: the SFINAE here uses .data() as a incomplete/imperfect proxy for the requirement - // on Container to be a contiguous sequence container. - template ::value && !details::is_std_array::value && - std::is_convertible::value && - std::is_convertible().data())>::value>> - constexpr span(Container& cont) : span(cont.data(), narrow(cont.size())) - {} - - template ::value && !details::is_span::value && - std::is_convertible::value && - std::is_convertible().data())>::value>> - constexpr span(const Container& cont) : span(cont.data(), narrow(cont.size())) - {} - - constexpr span(const span& other) noexcept = default; - - template < - class OtherElementType, std::ptrdiff_t OtherExtent, - class = std::enable_if_t< - details::is_allowed_extent_conversion::value && - details::is_allowed_element_type_conversion::value>> - constexpr span(const span& other) - : storage_(other.data(), details::extent_type(other.size())) - {} - - ~span() noexcept = default; - constexpr span& operator=(const span& other) noexcept = default; - - // [span.sub], span subviews - template - constexpr span first() const - { - Expects(Count >= 0 && Count <= size()); - return {data(), Count}; - } - - template - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr span last() const - { - Expects(Count >= 0 && size() - Count >= 0); - return {data() + (size() - Count), Count}; - } - - template - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr auto subspan() const -> - typename details::calculate_subspan_type::type - { - Expects((Offset >= 0 && size() - Offset >= 0) && - (Count == dynamic_extent || (Count >= 0 && Offset + Count <= size()))); - - return {data() + Offset, Count == dynamic_extent ? size() - Offset : Count}; - } - - constexpr span first(index_type count) const - { - Expects(count >= 0 && count <= size()); - return {data(), count}; - } - - constexpr span last(index_type count) const - { - return make_subspan(size() - count, dynamic_extent, subspan_selector{}); - } - - constexpr span subspan(index_type offset, - index_type count = dynamic_extent) const - { - return make_subspan(offset, count, subspan_selector{}); - } - - // [span.obs], span observers - constexpr index_type size() const noexcept { return storage_.size(); } - constexpr index_type size_bytes() const noexcept - { - return size() * narrow_cast(sizeof(element_type)); - } - constexpr bool empty() const noexcept { return size() == 0; } - - // [span.elem], span element access - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - constexpr reference operator[](index_type idx) const - { - Expects(CheckRange(idx, storage_.size())); - return data()[idx]; - } - - constexpr reference at(index_type idx) const { return this->operator[](idx); } - constexpr reference operator()(index_type idx) const { return this->operator[](idx); } - constexpr pointer data() const noexcept { return storage_.data(); } - - // [span.iter], span iterator support - constexpr iterator begin() const noexcept { return {this, 0}; } - constexpr iterator end() const noexcept { return {this, size()}; } - - constexpr const_iterator cbegin() const noexcept { return {this, 0}; } - constexpr const_iterator cend() const noexcept { return {this, size()}; } - - constexpr reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; } - constexpr reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; } - - constexpr const_reverse_iterator crbegin() const noexcept - { - return const_reverse_iterator{cend()}; - } - constexpr const_reverse_iterator crend() const noexcept - { - return const_reverse_iterator{cbegin()}; - } - -#ifdef _MSC_VER - // Tell MSVC how to unwrap spans in range-based-for - constexpr pointer _Unchecked_begin() const noexcept { return data(); } - constexpr pointer _Unchecked_end() const noexcept - { - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - return data() + size(); - } -#endif // _MSC_VER - -private: - static constexpr bool CheckRange(index_type idx, index_type size) noexcept - { - // Optimization: - // - // idx >= 0 && idx < size - // => - // static_cast(idx) < static_cast(size) - // - // because size >=0 by span construction, and negative idx will - // wrap around to a value always greater than size when casted. - - // check if we have enough space to wrap around -#if defined(__cpp_if_constexpr) - if constexpr (sizeof(index_type) <= sizeof(size_t)) -#else - if (sizeof(index_type) <= sizeof(size_t)) -#endif - { - return narrow_cast(idx) < narrow_cast(size); - } - else - { - return idx >= 0 && idx < size; - } - } - - // Needed to remove unnecessary null check in subspans - struct KnownNotNull - { - pointer p; - }; - - // this implementation detail class lets us take advantage of the - // empty base class optimization to pay for only storage of a single - // pointer in the case of fixed-size spans - template - class storage_type : public ExtentType - { - public: - // KnownNotNull parameter is needed to remove unnecessary null check - // in subspans and constructors from arrays - template - constexpr storage_type(KnownNotNull data, OtherExtentType ext) - : ExtentType(ext), data_(data.p) - { - Expects(ExtentType::size() >= 0); - } - - template - constexpr storage_type(pointer data, OtherExtentType ext) : ExtentType(ext), data_(data) - { - Expects(ExtentType::size() >= 0); - Expects(data || ExtentType::size() == 0); - } - - constexpr pointer data() const noexcept { return data_; } - - private: - pointer data_; - }; - - storage_type> storage_; - - // The rest is needed to remove unnecessary null check - // in subspans and constructors from arrays - constexpr span(KnownNotNull ptr, index_type count) : storage_(ptr, count) {} - - template - class subspan_selector - { - }; - - template - span make_subspan(index_type offset, index_type count, - subspan_selector) const - { - const span tmp(*this); - return tmp.subspan(offset, count); - } - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute - span make_subspan(index_type offset, index_type count, - subspan_selector) const - { - Expects(offset >= 0 && size() - offset >= 0); - - if (count == dynamic_extent) { return {KnownNotNull{data() + offset}, size() - offset}; } - - Expects(count >= 0 && size() - offset >= count); - return {KnownNotNull{data() + offset}, count}; - } -}; - -#if defined(GSL_USE_STATIC_CONSTEXPR_WORKAROUND) -template -constexpr const typename span::index_type span::extent; -#endif - -// [span.comparison], span comparison operators -template -constexpr bool operator==(span l, span r) -{ - return std::equal(l.begin(), l.end(), r.begin(), r.end()); -} - -template -constexpr bool operator!=(span l, span r) -{ - return !(l == r); -} - -template -constexpr bool operator<(span l, span r) -{ - return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); -} - -template -constexpr bool operator<=(span l, span r) -{ - return !(l > r); -} - -template -constexpr bool operator>(span l, span r) -{ - return r < l; -} - -template -constexpr bool operator>=(span l, span r) -{ - return !(l < r); -} - -namespace details -{ - // if we only supported compilers with good constexpr support then - // this pair of classes could collapse down to a constexpr function - - // we should use a narrow_cast<> to go to std::size_t, but older compilers may not see it as - // constexpr - // and so will fail compilation of the template - template - struct calculate_byte_size - : std::integral_constant(sizeof(ElementType) * - static_cast(Extent))> - { - }; - - template - struct calculate_byte_size - : std::integral_constant - { - }; -} // namespace details - -// [span.objectrep], views of object representation -template -span::value> -as_bytes(span s) noexcept -{ - GSL_SUPPRESS(type.1) // NO-FORMAT: attribute - return {reinterpret_cast(s.data()), s.size_bytes()}; -} - -template ::value>> -span::value> -as_writeable_bytes(span s) noexcept -{ - GSL_SUPPRESS(type.1) // NO-FORMAT: attribute - return {reinterpret_cast(s.data()), s.size_bytes()}; -} - -// -// make_span() - Utility functions for creating spans -// -template -constexpr span make_span(ElementType* ptr, - typename span::index_type count) -{ - return span(ptr, count); -} - -template -constexpr span make_span(ElementType* firstElem, ElementType* lastElem) -{ - return span(firstElem, lastElem); -} - -template -constexpr span make_span(ElementType (&arr)[N]) noexcept -{ - return span(arr); -} - -template -constexpr span make_span(Container& cont) -{ - return span(cont); -} - -template -constexpr span make_span(const Container& cont) -{ - return span(cont); -} - -template -constexpr span make_span(Ptr& cont, std::ptrdiff_t count) -{ - return span(cont, count); -} - -template -constexpr span make_span(Ptr& cont) -{ - return span(cont); -} - -// Specialization of gsl::at for span -template -constexpr ElementType& at(span s, index i) -{ - // No bounds checking here because it is done in span::operator[] called below - return s[i]; -} - -} // namespace gsl - -#if defined(_MSC_VER) && !defined(__clang__) -#if _MSC_VER < 1910 -#undef constexpr -#pragma pop_macro("constexpr") - -#endif // _MSC_VER < 1910 - -#pragma warning(pop) -#endif // _MSC_VER - -#if defined(__GNUC__) && __GNUC__ > 6 -#pragma GCC diagnostic pop -#endif // __GNUC__ > 6 - -#endif // GSL_SPAN_H diff --git a/src/algorithms/libs/gsl/include/gsl/string_span b/src/algorithms/libs/gsl/include/gsl/string_span deleted file mode 100644 index 85112e5d0..000000000 --- a/src/algorithms/libs/gsl/include/gsl/string_span +++ /dev/null @@ -1,722 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef GSL_STRING_SPAN_H -#define GSL_STRING_SPAN_H - -#include // for Ensures, Expects -#include // for narrow_cast -#include // for operator!=, operator==, dynamic_extent -#include // for not_null - -#include // for equal, lexicographical_compare -#include // for array -#include // for ptrdiff_t, size_t, nullptr_t -#include // for PTRDIFF_MAX -#include -#include // for basic_string, allocator, char_traits -#include // for declval, is_convertible, enable_if_t, add_... - -#if defined(_MSC_VER) && !defined(__clang__) -#pragma warning(push) - -// Turn MSVC /analyze rules that generate too much noise. TODO: fix in the tool. -#pragma warning(disable : 26446) // TODO: bug in parser - attributes and templates -#pragma warning(disable : 26481) // TODO: suppress does not work inside templates sometimes - -#if _MSC_VER < 1910 -#pragma push_macro("constexpr") -#define constexpr /*constexpr*/ - -#endif // _MSC_VER < 1910 -#endif // _MSC_VER - -namespace gsl -{ -// -// czstring and wzstring -// -// These are "tag" typedefs for C-style strings (i.e. null-terminated character arrays) -// that allow static analysis to help find bugs. -// -// There are no additional features/semantics that we can find a way to add inside the -// type system for these types that will not either incur significant runtime costs or -// (sometimes needlessly) break existing programs when introduced. -// - -template -using basic_zstring = CharT*; - -template -using czstring = basic_zstring; - -template -using cwzstring = basic_zstring; - -template -using cu16zstring = basic_zstring; - -template -using cu32zstring = basic_zstring; - -template -using zstring = basic_zstring; - -template -using wzstring = basic_zstring; - -template -using u16zstring = basic_zstring; - -template -using u32zstring = basic_zstring; - -namespace details -{ - template - std::ptrdiff_t string_length(const CharT* str, std::ptrdiff_t n) - { - if (str == nullptr || n <= 0) return 0; - - const span str_span{str, n}; - - std::ptrdiff_t len = 0; - while (len < n && str_span[len]) len++; - - return len; - } -} // namespace details - -// -// ensure_sentinel() -// -// Provides a way to obtain an span from a contiguous sequence -// that ends with a (non-inclusive) sentinel value. -// -// Will fail-fast if sentinel cannot be found before max elements are examined. -// -template -span ensure_sentinel(T* seq, std::ptrdiff_t max = PTRDIFF_MAX) -{ - Ensures(seq != nullptr); - - GSL_SUPPRESS(f.23) // NO-FORMAT: attribute // TODO: false positive // TODO: suppress does not work - auto cur = seq; - Ensures(cur != nullptr); // workaround for removing the warning - - GSL_SUPPRESS(bounds.1) // NO-FORMAT: attribute // TODO: suppress does not work - while ((cur - seq) < max && *cur != Sentinel) ++cur; - Ensures(*cur == Sentinel); - return {seq, cur - seq}; -} - -// -// ensure_z - creates a span for a zero terminated strings. -// Will fail fast if a null-terminator cannot be found before -// the limit of size_type. -// -template -span ensure_z(CharT* const& sz, std::ptrdiff_t max = PTRDIFF_MAX) -{ - return ensure_sentinel(sz, max); -} - -template -span ensure_z(CharT (&sz)[N]) -{ - return ensure_z(&sz[0], narrow_cast(N)); -} - -template -span::type, dynamic_extent> -ensure_z(Cont& cont) -{ - return ensure_z(cont.data(), narrow_cast(cont.size())); -} - -template -class basic_string_span; - -namespace details { - template - struct is_basic_string_span_oracle : std::false_type - { - }; - - template - struct is_basic_string_span_oracle> : std::true_type - { - }; - - template - struct is_basic_string_span : is_basic_string_span_oracle> - { - }; -} // namespace details - -// -// string_span and relatives -// -template -class basic_string_span -{ -public: - using element_type = CharT; - using value_type = std::remove_cv_t; - using pointer = std::add_pointer_t; - using reference = std::add_lvalue_reference_t; - using const_reference = std::add_lvalue_reference_t>; - using impl_type = span; - - using index_type = typename impl_type::index_type; - using iterator = typename impl_type::iterator; - using const_iterator = typename impl_type::const_iterator; - using reverse_iterator = typename impl_type::reverse_iterator; - using const_reverse_iterator = typename impl_type::const_reverse_iterator; - - using size_type = index_type; - - // default (empty) - constexpr basic_string_span() noexcept = default; - - // copy - constexpr basic_string_span(const basic_string_span& other) noexcept = default; - - // assign - constexpr basic_string_span& operator=(const basic_string_span& other) noexcept = default; - - constexpr basic_string_span(pointer ptr, index_type length) : span_(ptr, length) {} - constexpr basic_string_span(pointer firstElem, pointer lastElem) : span_(firstElem, lastElem) {} - - // From static arrays - if 0-terminated, remove 0 from the view - // All other containers allow 0s within the length, so we do not remove them - template - constexpr basic_string_span(element_type (&arr)[N]) : span_(remove_z(arr)) - {} - - template > - constexpr basic_string_span(std::array& arr) noexcept : span_(arr) - {} - - template > - constexpr basic_string_span(const std::array& arr) noexcept : span_(arr) - {} - - // Container signature should work for basic_string after C++17 version exists - template - // GSL_SUPPRESS(bounds.4) // NO-FORMAT: attribute // TODO: parser bug - constexpr basic_string_span(std::basic_string& str) - : span_(&str[0], narrow_cast(str.length())) - {} - - template - constexpr basic_string_span(const std::basic_string& str) - : span_(&str[0], str.length()) - {} - - // from containers. Containers must have a pointer type and data() function signatures - template ::value && - std::is_convertible::value && - std::is_convertible().data())>::value>> - constexpr basic_string_span(Container& cont) : span_(cont) - {} - - template ::value && - std::is_convertible::value && - std::is_convertible().data())>::value>> - constexpr basic_string_span(const Container& cont) : span_(cont) - {} - - // from string_span - template < - class OtherValueType, std::ptrdiff_t OtherExtent, - class = std::enable_if_t::impl_type, impl_type>::value>> - constexpr basic_string_span(basic_string_span other) - : span_(other.data(), other.length()) - {} - - template - constexpr basic_string_span first() const - { - return {span_.template first()}; - } - - constexpr basic_string_span first(index_type count) const - { - return {span_.first(count)}; - } - - template - constexpr basic_string_span last() const - { - return {span_.template last()}; - } - - constexpr basic_string_span last(index_type count) const - { - return {span_.last(count)}; - } - - template - constexpr basic_string_span subspan() const - { - return {span_.template subspan()}; - } - - constexpr basic_string_span - subspan(index_type offset, index_type count = dynamic_extent) const - { - return {span_.subspan(offset, count)}; - } - - constexpr reference operator[](index_type idx) const { return span_[idx]; } - constexpr reference operator()(index_type idx) const { return span_[idx]; } - - constexpr pointer data() const { return span_.data(); } - - constexpr index_type length() const noexcept { return span_.size(); } - constexpr index_type size() const noexcept { return span_.size(); } - constexpr index_type size_bytes() const noexcept { return span_.size_bytes(); } - constexpr index_type length_bytes() const noexcept { return span_.length_bytes(); } - constexpr bool empty() const noexcept { return size() == 0; } - - constexpr iterator begin() const noexcept { return span_.begin(); } - constexpr iterator end() const noexcept { return span_.end(); } - - constexpr const_iterator cbegin() const noexcept { return span_.cbegin(); } - constexpr const_iterator cend() const noexcept { return span_.cend(); } - - constexpr reverse_iterator rbegin() const noexcept { return span_.rbegin(); } - constexpr reverse_iterator rend() const noexcept { return span_.rend(); } - - constexpr const_reverse_iterator crbegin() const noexcept { return span_.crbegin(); } - constexpr const_reverse_iterator crend() const noexcept { return span_.crend(); } - -private: - static impl_type remove_z(pointer const& sz, std::ptrdiff_t max) - { - return {sz, details::string_length(sz, max)}; - } - - template - static impl_type remove_z(element_type (&sz)[N]) - { - return remove_z(&sz[0], narrow_cast(N)); - } - - impl_type span_; -}; - -template -using string_span = basic_string_span; - -template -using cstring_span = basic_string_span; - -template -using wstring_span = basic_string_span; - -template -using cwstring_span = basic_string_span; - -template -using u16string_span = basic_string_span; - -template -using cu16string_span = basic_string_span; - -template -using u32string_span = basic_string_span; - -template -using cu32string_span = basic_string_span; - -// -// to_string() allow (explicit) conversions from string_span to string -// - -template -std::basic_string::type> -to_string(basic_string_span view) -{ - return {view.data(), narrow_cast(view.length())}; -} - -template , - typename Allocator = std::allocator, typename gCharT, std::ptrdiff_t Extent> -std::basic_string to_basic_string(basic_string_span view) -{ - return {view.data(), narrow_cast(view.length())}; -} - -template -basic_string_span::value> -as_bytes(basic_string_span s) noexcept -{ - GSL_SUPPRESS(type.1) // NO-FORMAT: attribute - return {reinterpret_cast(s.data()), s.size_bytes()}; -} - -template ::value>> -basic_string_span::value> -as_writeable_bytes(basic_string_span s) noexcept -{ - GSL_SUPPRESS(type.1) // NO-FORMAT: attribute - return {reinterpret_cast(s.data()), s.size_bytes()}; -} - -// zero-terminated string span, used to convert -// zero-terminated spans to legacy strings -template -class basic_zstring_span { -public: - using value_type = CharT; - using const_value_type = std::add_const_t; - - using pointer = std::add_pointer_t; - using const_pointer = std::add_pointer_t; - - using zstring_type = basic_zstring; - using const_zstring_type = basic_zstring; - - using impl_type = span; - using string_span_type = basic_string_span; - - constexpr basic_zstring_span(impl_type s) : span_(s) - { - // expects a zero-terminated span - Expects(s[s.size() - 1] == '\0'); - } - - // copy - constexpr basic_zstring_span(const basic_zstring_span& other) = default; - - // move - constexpr basic_zstring_span(basic_zstring_span&& other) = default; - - // assign - constexpr basic_zstring_span& operator=(const basic_zstring_span& other) = default; - - // move assign - constexpr basic_zstring_span& operator=(basic_zstring_span&& other) = default; - - constexpr bool empty() const noexcept { return span_.size() == 0; } - - constexpr string_span_type as_string_span() const noexcept - { - const auto sz = span_.size(); - return {span_.data(), sz > 1 ? sz - 1 : 0}; - } - constexpr string_span_type ensure_z() const { return gsl::ensure_z(span_); } - - constexpr const_zstring_type assume_z() const noexcept { return span_.data(); } - -private: - impl_type span_; -}; - -template -using zstring_span = basic_zstring_span; - -template -using wzstring_span = basic_zstring_span; - -template -using u16zstring_span = basic_zstring_span; - -template -using u32zstring_span = basic_zstring_span; - -template -using czstring_span = basic_zstring_span; - -template -using cwzstring_span = basic_zstring_span; - -template -using cu16zstring_span = basic_zstring_span; - -template -using cu32zstring_span = basic_zstring_span; - -// operator == -template ::value || - std::is_convertible>>::value>> -bool operator==(const gsl::basic_string_span& one, const T& other) -{ - const gsl::basic_string_span> tmp(other); - return std::equal(one.begin(), one.end(), tmp.begin(), tmp.end()); -} - -template ::value && - std::is_convertible>>::value>> -bool operator==(const T& one, const gsl::basic_string_span& other) -{ - const gsl::basic_string_span> tmp(one); - return std::equal(tmp.begin(), tmp.end(), other.begin(), other.end()); -} - -// operator != -template , Extent>>::value>> -bool operator!=(gsl::basic_string_span one, const T& other) -{ - return !(one == other); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename = std::enable_if_t< - std::is_convertible, Extent>>::value && - !gsl::details::is_basic_string_span::value>> -bool operator!=(const T& one, gsl::basic_string_span other) -{ - return !(one == other); -} - -// operator< -template , Extent>>::value>> -bool operator<(gsl::basic_string_span one, const T& other) -{ - const gsl::basic_string_span, Extent> tmp(other); - return std::lexicographical_compare(one.begin(), one.end(), tmp.begin(), tmp.end()); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename = std::enable_if_t< - std::is_convertible, Extent>>::value && - !gsl::details::is_basic_string_span::value>> -bool operator<(const T& one, gsl::basic_string_span other) -{ - gsl::basic_string_span, Extent> tmp(one); - return std::lexicographical_compare(tmp.begin(), tmp.end(), other.begin(), other.end()); -} - -#ifndef _MSC_VER - -// VS treats temp and const containers as convertible to basic_string_span, -// so the cases below are already covered by the previous operators - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator<(gsl::basic_string_span one, const T& other) -{ - gsl::basic_string_span, Extent> tmp(other); - return std::lexicographical_compare(one.begin(), one.end(), tmp.begin(), tmp.end()); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator<(const T& one, gsl::basic_string_span other) -{ - gsl::basic_string_span, Extent> tmp(one); - return std::lexicographical_compare(tmp.begin(), tmp.end(), other.begin(), other.end()); -} -#endif - -// operator <= -template , Extent>>::value>> -bool operator<=(gsl::basic_string_span one, const T& other) -{ - return !(other < one); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename = std::enable_if_t< - std::is_convertible, Extent>>::value && - !gsl::details::is_basic_string_span::value>> -bool operator<=(const T& one, gsl::basic_string_span other) -{ - return !(other < one); -} - -#ifndef _MSC_VER - -// VS treats temp and const containers as convertible to basic_string_span, -// so the cases below are already covered by the previous operators - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator<=(gsl::basic_string_span one, const T& other) -{ - return !(other < one); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator<=(const T& one, gsl::basic_string_span other) -{ - return !(other < one); -} -#endif - -// operator> -template , Extent>>::value>> -bool operator>(gsl::basic_string_span one, const T& other) -{ - return other < one; -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename = std::enable_if_t< - std::is_convertible, Extent>>::value && - !gsl::details::is_basic_string_span::value>> -bool operator>(const T& one, gsl::basic_string_span other) -{ - return other < one; -} - -#ifndef _MSC_VER - -// VS treats temp and const containers as convertible to basic_string_span, -// so the cases below are already covered by the previous operators - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator>(gsl::basic_string_span one, const T& other) -{ - return other < one; -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator>(const T& one, gsl::basic_string_span other) -{ - return other < one; -} -#endif - -// operator >= -template , Extent>>::value>> -bool operator>=(gsl::basic_string_span one, const T& other) -{ - return !(one < other); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename = std::enable_if_t< - std::is_convertible, Extent>>::value && - !gsl::details::is_basic_string_span::value>> -bool operator>=(const T& one, gsl::basic_string_span other) -{ - return !(one < other); -} - -#ifndef _MSC_VER - -// VS treats temp and const containers as convertible to basic_string_span, -// so the cases below are already covered by the previous operators - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator>=(gsl::basic_string_span one, const T& other) -{ - return !(one < other); -} - -template < - typename CharT, std::ptrdiff_t Extent = gsl::dynamic_extent, typename T, - typename DataType = typename T::value_type, - typename = std::enable_if_t< - !gsl::details::is_span::value && !gsl::details::is_basic_string_span::value && - std::is_convertible::value && - std::is_same().size(), *std::declval().data())>, - DataType>::value>> -bool operator>=(const T& one, gsl::basic_string_span other) -{ - return !(one < other); -} -#endif -} // namespace gsl - -#if defined(_MSC_VER) && !defined(__clang__) -#pragma warning(pop) - -#if _MSC_VER < 1910 -#undef constexpr -#pragma pop_macro("constexpr") - -#endif // _MSC_VER < 1910 -#endif // _MSC_VER - -#endif // GSL_STRING_SPAN_H From dd53f81b1a44e05fba9afc40e0e42e30233633fd Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 11:58:08 +0200 Subject: [PATCH 07/64] Replace memcpy by copy_n. Avoid pointer arithmetics --- .../adapters/beidou_b1i_pcps_acquisition.cc | 7 ++++--- .../adapters/beidou_b3i_pcps_acquisition.cc | 6 +++--- .../galileo_e1_pcps_8ms_ambiguous_acquisition.cc | 5 +++-- .../galileo_e1_pcps_ambiguous_acquisition.cc | 5 +++-- .../galileo_e1_pcps_ambiguous_acquisition_fpga.cc | 8 ++++---- ...leo_e1_pcps_quicksync_ambiguous_acquisition.cc | 7 +++---- .../galileo_e1_pcps_tong_ambiguous_acquisition.cc | 5 +++-- .../galileo_e5a_noncoherent_iq_acquisition_caf.cc | 15 +++++++-------- .../adapters/galileo_e5a_pcps_acquisition.cc | 5 +++-- .../adapters/galileo_e5a_pcps_acquisition_fpga.cc | 8 ++++---- .../adapters/glonass_l1_ca_pcps_acquisition.cc | 5 +++-- .../adapters/glonass_l2_ca_pcps_acquisition.cc | 5 +++-- .../adapters/gps_l1_ca_pcps_acquisition.cc | 5 +++-- .../adapters/gps_l1_ca_pcps_acquisition_fpga.cc | 8 ++++---- .../adapters/gps_l1_ca_pcps_opencl_acquisition.cc | 5 +++-- .../gps_l1_ca_pcps_quicksync_acquisition.cc | 8 ++++---- .../adapters/gps_l1_ca_pcps_tong_acquisition.cc | 5 +++-- .../adapters/gps_l2_m_pcps_acquisition.cc | 5 +++-- .../adapters/gps_l2_m_pcps_acquisition_fpga.cc | 8 ++++---- .../adapters/gps_l5i_pcps_acquisition.cc | 7 ++++--- .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 8 ++++---- 21 files changed, 75 insertions(+), 65 deletions(-) diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc index c4d188771..c911af3d4 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc @@ -39,6 +39,7 @@ #include "gnss_sdr_flags.h" #include #include +#include BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( @@ -208,10 +209,10 @@ void BeidouB1iPcpsAcquisition::set_local_code() beidou_b1i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - for (uint32_t i = 0; i < sampled_ms_; i++) + gsl::span code_span(code_, vector_length_); + for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc index f92b61572..b6bf5f7ed 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc @@ -37,7 +37,7 @@ #include "gnss_sdr_flags.h" #include #include - +#include using google::LogMessage; @@ -206,10 +206,10 @@ void BeidouB3iPcpsAcquisition::set_local_code() beidou_b3i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc index ddfeb7b8a..330016bd2 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc @@ -36,6 +36,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition( @@ -223,10 +224,10 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_local_code() galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_cc_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index d419cd0fb..f0cfce6a3 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -37,6 +37,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( @@ -275,10 +276,10 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() } } - + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { - memcpy(&(code_[i * code_length_]), code, sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index b781319b0..24e275ff6 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -40,9 +40,9 @@ #include // for gr_complex #include // for volk_32fc_conjugate_32fc #include -#include // for abs, pow, floor -#include // for complex -#include // for memcpy +#include // for copy_n +#include // for abs, pow, floor +#include // for complex // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. @@ -144,7 +144,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( code[s] = std::complex(0.0, 0.0); } - memcpy(fft_if->get_inbuf(), code, sizeof(gr_complex) * nsamples_total); // copy to FFT buffer + std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc index 8bc3f969c..4e4c06ba9 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc @@ -36,6 +36,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcquisition( @@ -257,14 +258,12 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code() galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); - + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < (sampled_ms_ / (folding_factor_ * 4)); i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - // memcpy(code_, code,sizeof(gr_complex)*code_length_); acquisition_cc_->set_local_code(code_); delete[] code; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc index 7728988e7..4ab468fe8 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc @@ -36,6 +36,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition( @@ -226,10 +227,10 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_local_code() galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_cc_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc index f4eb2024a..e12e893e2 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc @@ -42,6 +42,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf( @@ -244,28 +245,26 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_local_code() } // WARNING: 3ms are coherently integrated. Secondary sequence (1,1,1) // is generated, and modulated in the 'block'. + gsl::span codeQ_span(codeQ_, vector_length_); + gsl::span codeI_span(codeI_, vector_length_); if (Zero_padding == 0) // if no zero_padding { for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(codeI_[i * code_length_]), codeI, - sizeof(gr_complex) * code_length_); + std::copy_n(codeI, code_length_, codeI_span.subspan(i * code_length_, code_length_).data()); if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') { - memcpy(&(codeQ_[i * code_length_]), codeQ, - sizeof(gr_complex) * code_length_); + std::copy_n(codeQ, code_length_, codeQ_span.subspan(i * code_length_, code_length_).data()); } } } else { // 1ms code + 1ms zero padding - memcpy(&(codeI_[0]), codeI, - sizeof(gr_complex) * code_length_); + std::copy_n(codeI, code_length_, codeI_); if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') { - memcpy(&(codeQ_[0]), codeQ, - sizeof(gr_complex) * code_length_); + std::copy_n(codeQ, code_length_, codeQ_); } } diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc index 5b42ff2f2..7da2a1bd6 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc @@ -37,6 +37,7 @@ #include #include #include +#include GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* configuration, @@ -261,10 +262,10 @@ void GalileoE5aPcpsAcquisition::set_local_code() { galileo_e5_a_code_gen_complex_sampled(gsl::span(code, code_length_), signal_, gnss_synchro_->PRN, fs_in_, 0); } - + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(code_ + (i * code_length_), code, sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index 97a14f2a9..15361509f 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -40,9 +40,9 @@ #include // for gr_complex #include // for volk_32fc_conjugate_32fc #include -#include // for abs, pow, floor -#include // for complex -#include // for strcpy, memcpy +#include // for copy_n +#include // for abs, pow, floor +#include // for complex // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. @@ -149,7 +149,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf code[s] = std::complex(0.0, 0.0); } - memcpy(fft_if->get_inbuf(), code, sizeof(gr_complex) * nsamples_total); // copy to FFT buffer + std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc index a375d01f7..45c7645bb 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -39,6 +39,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( @@ -210,10 +211,10 @@ void GlonassL1CaPcpsAcquisition::set_local_code() glonass_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), /* gnss_synchro_->PRN,*/ fs_in_, 0); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc index 0fd369a9b..217f31d31 100644 --- a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc @@ -38,6 +38,7 @@ #include "gnss_sdr_flags.h" #include #include +#include GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition( @@ -210,10 +211,10 @@ void GlonassL2CaPcpsAcquisition::set_local_code() glonass_l2_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), /* gnss_synchro_->PRN,*/ fs_in_, 0); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc index 4e458ebcc..297464005 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc @@ -41,6 +41,7 @@ #include "gps_sdr_signal_processing.h" #include #include +#include #include @@ -237,10 +238,10 @@ void GpsL1CaPcpsAcquisition::set_local_code() { gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); } + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index b13f7ac23..7c6d7ad2d 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -43,9 +43,9 @@ #include // for gr_complex #include // for volk_32fc_conjugate_32fc #include -#include // for abs, pow, floor -#include // for complex -#include // for memcpy +#include // for copy_n +#include // for abs, pow, floor +#include // for complex #define NUM_PRNs 32 @@ -126,7 +126,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( code[s] = std::complex(0.0, 0.0); } - memcpy(fft_if->get_inbuf(), code, sizeof(gr_complex) * nsamples_total); // copy to FFT buffer + std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc index b0fb13060..f96a29632 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc @@ -36,6 +36,7 @@ #include "gps_sdr_signal_processing.h" #include #include +#include GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition( @@ -211,10 +212,10 @@ void GpsL1CaPcpsOpenClAcquisition::set_local_code() gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_cc_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc index b1b0034f4..d28420e7c 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc @@ -37,6 +37,7 @@ #include "gps_sdr_signal_processing.h" #include #include +#include GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition( @@ -240,13 +241,12 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_local_code() gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - for (unsigned int i = 0; i < (sampled_ms_ / folding_factor_); i++) + gsl::span code_span(code_, vector_length_); + for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - //memcpy(code_, code,sizeof(gr_complex)*code_length_); acquisition_cc_->set_local_code(code_); delete[] code; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc index 7b56356bb..731bd2e77 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc @@ -36,6 +36,7 @@ #include "gps_sdr_signal_processing.h" #include #include +#include GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition( @@ -201,10 +202,10 @@ void GpsL1CaPcpsTongAcquisition::set_local_code() gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_cc_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc index d92c49102..2777d8440 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc @@ -39,6 +39,7 @@ #include "gps_l2c_signal.h" #include #include +#include GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition( @@ -250,10 +251,10 @@ void GpsL2MPcpsAcquisition::set_local_code() gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_); } + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < num_codes_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index e369189e9..dcbbd331a 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -42,9 +42,9 @@ #include // for gr_complex #include // for volk_32fc_conjugate_32fc #include -#include // for abs, pow, floor -#include // for complex -#include // for memcpy +#include // for copy_n +#include // for abs, pow, floor +#include // for complex #define NUM_PRNs 32 #define QUANT_BITS_LOCAL_CODE 16 @@ -119,7 +119,7 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( { code[s] = std::complex(0.0, 0.0); } - memcpy(fft_if->get_inbuf(), code, sizeof(gr_complex) * nsamples_total); // copy to FFT buffer + std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values max = 0; // initialize maximum value diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc index 204d5532e..3c30f8e5f 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc @@ -39,6 +39,7 @@ #include "gps_l5_signal.h" #include #include +#include GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( @@ -230,11 +231,11 @@ void GpsL5iPcpsAcquisition::init() acquisition_->init(); } + void GpsL5iPcpsAcquisition::set_local_code() { auto* code = new std::complex[code_length_]; - if (acq_parameters_.use_automatic_resampler) { gps_l5i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); @@ -244,10 +245,10 @@ void GpsL5iPcpsAcquisition::set_local_code() gps_l5i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_); } + gsl::span code_span(code_, vector_length_); for (unsigned int i = 0; i < num_codes_; i++) { - memcpy(&(code_[i * code_length_]), code, - sizeof(gr_complex) * code_length_); + std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } acquisition_->set_local_code(code_); diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index 072abefb4..118fdf651 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -43,9 +43,9 @@ #include // for gr_complex #include // for volk_32fc_conjugate_32fc #include -#include // for abs, pow, floor -#include // for complex -#include // for memcpy +#include // for copy_n +#include // for abs, pow, floor +#include // for complex #define NUM_PRNs 32 @@ -130,7 +130,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( // fill in zero padding code[s] = std::complex(0.0, 0.0); } - memcpy(fft_if->get_inbuf(), code, sizeof(gr_complex) * nsamples_total); // copy to FFT buffer + std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values From d6714e35a1dd3e3ef7f637601c908e957e06bccf Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 12:55:39 +0200 Subject: [PATCH 08/64] Avoid pointer arithmetics --- .../libs/galileo_e1_signal_processing.cc | 23 ++-- .../libs/galileo_e5_signal_processing.cc | 8 +- src/algorithms/libs/gnss_signal_processing.cc | 130 +++++++++--------- src/algorithms/libs/gnss_signal_processing.h | 2 +- src/algorithms/libs/gps_l2c_signal.cc | 23 ++-- src/algorithms/libs/gps_l5_signal.cc | 38 ++--- 6 files changed, 112 insertions(+), 112 deletions(-) diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index 2b4a6a527..e594662fb 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -53,7 +53,7 @@ void galileo_e1_code_gen_int(gsl::span _dest, std::array _Signal, { for (char i : GALILEO_E1_B_PRIMARY_CODE[prn]) { - hex_to_binary_converter(&_dest[index], i); + hex_to_binary_converter(_dest.subspan(index, 4), i); index += 4; } } @@ -61,7 +61,7 @@ void galileo_e1_code_gen_int(gsl::span _dest, std::array _Signal, { for (char i : GALILEO_E1_C_PRIMARY_CODE[prn]) { - hex_to_binary_converter(&_dest[index], i); + hex_to_binary_converter(_dest.subspan(index, 4), i); index += 4; } } @@ -175,7 +175,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _signal_E1_span(_signal_E1, _codeLength); if (_cboc == true) { galileo_e1_gen_float(gsl::span(_signal_E1, _codeLength), gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)), _Signal); // generate cboc 12 samples per chip @@ -183,11 +183,12 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array(volk_gnsssdr_malloc(_codeLength * sizeof(int32_t), volk_gnsssdr_get_alignment())); + gsl::span _signal_E1_int_span(_signal_E1_int, _codeLength); galileo_e1_sinboc_11_gen_int(gsl::span(_signal_E1_int, _codeLength), gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS))); // generate sinboc(1,1) 2 samples per chip for (uint32_t ii = 0; ii < _codeLength; ++ii) { - _signal_E1[ii] = static_cast(_signal_E1_int[ii]); + _signal_E1_span[ii] = static_cast(_signal_E1_int_span[ii]); } volk_gnsssdr_free(_signal_E1_int); } @@ -205,24 +206,24 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array= 2 && _secondary_flag) { auto* _signal_E1C_secondary = new float[static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode]; - + gsl::span _signal_E1C_secondary_span(_signal_E1C_secondary, static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode); for (uint32_t i = 0; i < static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH); i++) { for (unsigned k = 0; k < _samplesPerCode; k++) { - _signal_E1C_secondary[i * _samplesPerCode + k] = _signal_E1[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); + _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); } } _samplesPerCode *= static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH); delete[] _signal_E1; - _signal_E1 = _signal_E1C_secondary; + _signal_E1 = _signal_E1C_secondary_span.data(); } for (uint32_t i = 0; i < _samplesPerCode; i++) { - _dest[(i + delay) % _samplesPerCode] = _signal_E1[i]; + _dest[(i + delay) % _samplesPerCode] = _signal_E1_span[i]; } delete[] _signal_E1; @@ -245,12 +246,12 @@ void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, s } auto* real_code = static_cast(volk_gnsssdr_malloc(_samplesPerCode * sizeof(float), volk_gnsssdr_get_alignment())); - - galileo_e1_code_gen_float_sampled(gsl::span(real_code, _samplesPerCode), _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag); + gsl::span real_code_span(real_code, _samplesPerCode); + galileo_e1_code_gen_float_sampled(real_code_span, _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag); for (uint32_t ii = 0; ii < _samplesPerCode; ++ii) { - _dest[ii] = std::complex(real_code[ii], 0.0f); + _dest[ii] = std::complex(real_code_span[ii], 0.0f); } volk_gnsssdr_free(real_code); } diff --git a/src/algorithms/libs/galileo_e5_signal_processing.cc b/src/algorithms/libs/galileo_e5_signal_processing.cc index f9a7a0442..5df87e416 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.cc +++ b/src/algorithms/libs/galileo_e5_signal_processing.cc @@ -109,8 +109,8 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, const int32_t _codeFreqBasis = GALILEO_E5A_CODE_CHIP_RATE_HZ; auto* _code = new std::complex[_codeLength](); - - galileo_e5_a_code_gen_complex_primary(gsl::span>(_code, _codeLength), _prn, _Signal); + gsl::span> _code_span(_code, _codeLength); + galileo_e5_a_code_gen_complex_primary(_code_span, _prn, _Signal); _samplesPerCode = static_cast(static_cast(_fs) / (static_cast(_codeFreqBasis) / static_cast(_codeLength))); @@ -122,14 +122,14 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, if (posix_memalign(reinterpret_cast(&_resampled_signal), 16, _samplesPerCode * sizeof(gr_complex)) == 0) { }; - resampler(gsl::span>(_code, _codeLength), gsl::span>(_resampled_signal, _samplesPerCode), _codeFreqBasis, _fs); // resamples code to fs + resampler(_code_span, gsl::span>(_resampled_signal, _samplesPerCode), _codeFreqBasis, _fs); // resamples code to fs delete[] _code; _code = _resampled_signal; } for (uint32_t i = 0; i < _samplesPerCode; i++) { - _dest[(i + delay) % _samplesPerCode] = _code[i]; + _dest[(i + delay) % _samplesPerCode] = _code_span[i]; } if (_fs != _codeFreqBasis) { diff --git a/src/algorithms/libs/gnss_signal_processing.cc b/src/algorithms/libs/gnss_signal_processing.cc index b90007451..71e61e159 100644 --- a/src/algorithms/libs/gnss_signal_processing.cc +++ b/src/algorithms/libs/gnss_signal_processing.cc @@ -54,105 +54,105 @@ void complex_exp_gen_conj(gsl::span> _dest, double _f, doubl } -void hex_to_binary_converter(int32_t* _dest, char _from) +void hex_to_binary_converter(gsl::span _dest, char _from) { switch (_from) { case '0': - *(_dest) = 1; - *(_dest + 1) = 1; - *(_dest + 2) = 1; - *(_dest + 3) = 1; + _dest[0] = 1; + _dest[1] = 1; + _dest[2] = 1; + _dest[3] = 1; break; case '1': - *(_dest) = 1; - *(_dest + 1) = 1; - *(_dest + 2) = 1; - *(_dest + 3) = -1; + _dest[0] = 1; + _dest[1] = 1; + _dest[2] = 1; + _dest[3] = -1; break; case '2': - *(_dest) = 1; - *(_dest + 1) = 1; - *(_dest + 2) = -1; - *(_dest + 3) = 1; + _dest[0] = 1; + _dest[1] = 1; + _dest[2] = -1; + _dest[3] = 1; break; case '3': - *(_dest) = 1; - *(_dest + 1) = 1; - *(_dest + 2) = -1; - *(_dest + 3) = -1; + _dest[0] = 1; + _dest[1] = 1; + _dest[2] = -1; + _dest[3] = -1; break; case '4': - *(_dest) = 1; - *(_dest + 1) = -1; - *(_dest + 2) = 1; - *(_dest + 3) = 1; + _dest[0] = 1; + _dest[1] = -1; + _dest[2] = 1; + _dest[3] = 1; break; case '5': - *(_dest) = 1; - *(_dest + 1) = -1; - *(_dest + 2) = 1; - *(_dest + 3) = -1; + _dest[0] = 1; + _dest[1] = -1; + _dest[2] = 1; + _dest[3] = -1; break; case '6': - *(_dest) = 1; - *(_dest + 1) = -1; - *(_dest + 2) = -1; - *(_dest + 3) = 1; + _dest[0] = 1; + _dest[1] = -1; + _dest[2] = -1; + _dest[3] = 1; break; case '7': - *(_dest) = 1; - *(_dest + 1) = -1; - *(_dest + 2) = -1; - *(_dest + 3) = -1; + _dest[0] = 1; + _dest[1] = -1; + _dest[2] = -1; + _dest[3] = -1; break; case '8': - *(_dest) = -1; - *(_dest + 1) = 1; - *(_dest + 2) = 1; - *(_dest + 3) = 1; + _dest[0] = -1; + _dest[1] = 1; + _dest[2] = 1; + _dest[3] = 1; break; case '9': - *(_dest) = -1; - *(_dest + 1) = 1; - *(_dest + 2) = 1; - *(_dest + 3) = -1; + _dest[0] = -1; + _dest[1] = 1; + _dest[2] = 1; + _dest[3] = -1; break; case 'A': - *(_dest) = -1; - *(_dest + 1) = 1; - *(_dest + 2) = -1; - *(_dest + 3) = 1; + _dest[0] = -1; + _dest[1] = 1; + _dest[2] = -1; + _dest[3] = 1; break; case 'B': - *(_dest) = -1; - *(_dest + 1) = 1; - *(_dest + 2) = -1; - *(_dest + 3) = -1; + _dest[0] = -1; + _dest[1] = 1; + _dest[2] = -1; + _dest[3] = -1; break; case 'C': - *(_dest) = -1; - *(_dest + 1) = -1; - *(_dest + 2) = 1; - *(_dest + 3) = 1; + _dest[0] = -1; + _dest[1] = -1; + _dest[2] = 1; + _dest[3] = 1; break; case 'D': - *(_dest) = -1; - *(_dest + 1) = -1; - *(_dest + 2) = 1; - *(_dest + 3) = -1; + _dest[0] = -1; + _dest[1] = -1; + _dest[2] = 1; + _dest[3] = -1; break; case 'E': - *(_dest) = -1; - *(_dest + 1) = -1; - *(_dest + 2) = -1; - *(_dest + 3) = 1; + _dest[0] = -1; + _dest[1] = -1; + _dest[2] = -1; + _dest[3] = 1; break; case 'F': - *(_dest) = -1; - *(_dest + 1) = -1; - *(_dest + 2) = -1; - *(_dest + 3) = -1; + _dest[0] = -1; + _dest[1] = -1; + _dest[2] = -1; + _dest[3] = -1; break; } } diff --git a/src/algorithms/libs/gnss_signal_processing.h b/src/algorithms/libs/gnss_signal_processing.h index ba82a542b..29d00c00e 100644 --- a/src/algorithms/libs/gnss_signal_processing.h +++ b/src/algorithms/libs/gnss_signal_processing.h @@ -63,7 +63,7 @@ void complex_exp_gen_conj(gsl::span> _dest, double _f, doubl * to binary (the output are 4 ints with +1 or -1 values). * */ -void hex_to_binary_converter(int32_t* _dest, char _from); +void hex_to_binary_converter(gsl::span _dest, char _from); /*! * \brief This function resamples a sequence of float values. diff --git a/src/algorithms/libs/gps_l2c_signal.cc b/src/algorithms/libs/gps_l2c_signal.cc index e452072a5..2708926f7 100644 --- a/src/algorithms/libs/gps_l2c_signal.cc +++ b/src/algorithms/libs/gps_l2c_signal.cc @@ -56,15 +56,15 @@ void gps_l2c_m_code(gsl::span _dest, uint32_t _prn) void gps_l2c_m_code_gen_complex(gsl::span> _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; - + gsl::span _code_span(_code, GPS_L2_M_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - gps_l2c_m_code(gsl::span(_code, GPS_L2_M_CODE_LENGTH_CHIPS), _prn); + gps_l2c_m_code(_code_span, _prn); } for (int32_t i = 0; i < GPS_L2_M_CODE_LENGTH_CHIPS; i++) { - _dest[i] = std::complex(1.0 - 2.0 * _code[i], 0.0); + _dest[i] = std::complex(1.0 - 2.0 * _code_span[i], 0.0); } delete[] _code; @@ -74,15 +74,15 @@ void gps_l2c_m_code_gen_complex(gsl::span> _dest, uint32_t _ void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; - + gsl::span _code_span(_code, GPS_L2_M_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - gps_l2c_m_code(gsl::span(_code, GPS_L2_M_CODE_LENGTH_CHIPS), _prn); + gps_l2c_m_code(_code_span, _prn); } for (int32_t i = 0; i < GPS_L2_M_CODE_LENGTH_CHIPS; i++) { - _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + _dest[i] = 1.0 - 2.0 * static_cast(_code_span[i]); } delete[] _code; @@ -95,9 +95,10 @@ void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn) void gps_l2c_m_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; + gsl::span _code_span(_code, GPS_L2_M_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - gps_l2c_m_code(gsl::span(_code, GPS_L2_M_CODE_LENGTH_CHIPS), _prn); + gps_l2c_m_code(_code_span, _prn); } int32_t _samplesPerCode, _codeValueIndex; @@ -112,26 +113,22 @@ void gps_l2c_m_code_gen_complex_sampled(gsl::span> _dest, ui _ts = 1.0 / static_cast(_fs); // Sampling period in sec _tc = 1.0 / static_cast(GPS_L2_M_CODE_RATE_HZ); // C/A chip period in sec - //float aux; for (int32_t i = 0; i < _samplesPerCode; i++) { //=== Digitizing ======================================================= //--- Make index array to read L2C code values ------------------------- - //TODO: Check this formula! Seems to start with an extra sample _codeValueIndex = std::ceil((_ts * (static_cast(i) + 1)) / _tc) - 1; - //aux = (_ts * (i + 1)) / _tc; - //_codeValueIndex = static_cast(static_cast(aux)) - 1; //--- Make the digitized version of the L2C code ----------------------- if (i == _samplesPerCode - 1) { //--- Correct the last index (due to number rounding issues) ----------- - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeLength - 1], 0); + _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeLength - 1], 0); } else { - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample + _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeValueIndex], 0); //repeat the chip -> upsample } } delete[] _code; diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index 9383ec2b2..9a9b89e03 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -174,15 +174,15 @@ void make_l5q(gsl::span _dest, int32_t prn) void gps_l5i_code_gen_complex(gsl::span> _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; - + gsl::span _code_span(_code, GPS_L5I_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - make_l5i(gsl::span(_code, GPS_L5I_CODE_LENGTH_CHIPS), _prn - 1); + make_l5i(_code_span, _prn - 1); } for (int32_t i = 0; i < GPS_L5I_CODE_LENGTH_CHIPS; i++) { - _dest[i] = std::complex(1.0 - 2.0 * _code[i], 0.0); + _dest[i] = std::complex(1.0 - 2.0 * _code_span[i], 0.0); } delete[] _code; @@ -192,15 +192,15 @@ void gps_l5i_code_gen_complex(gsl::span> _dest, uint32_t _pr void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; - + gsl::span _code_span(_code, GPS_L5I_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - make_l5i(gsl::span(_code, GPS_L5I_CODE_LENGTH_CHIPS), _prn - 1); + make_l5i(_code_span, _prn - 1); } for (int32_t i = 0; i < GPS_L5I_CODE_LENGTH_CHIPS; i++) { - _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + _dest[i] = 1.0 - 2.0 * static_cast(_code_span[i]); } delete[] _code; @@ -213,9 +213,10 @@ void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn) void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; + gsl::span _code_span(_code, GPS_L5I_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - make_l5i(gsl::span(_code, GPS_L5I_CODE_LENGTH_CHIPS), _prn - 1); + make_l5i(_code_span, _prn - 1); } int32_t _samplesPerCode, _codeValueIndex; @@ -241,11 +242,11 @@ void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint if (i == _samplesPerCode - 1) { //--- Correct the last index (due to number rounding issues) ----------- - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeLength - 1], 0.0); + _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeLength - 1], 0.0); } else { - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0.0); // repeat the chip -> upsample + _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeValueIndex], 0.0); // repeat the chip -> upsample } } delete[] _code; @@ -255,15 +256,15 @@ void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint void gps_l5q_code_gen_complex(gsl::span> _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; - + gsl::span _code_span(_code, GPS_L5Q_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - make_l5q(gsl::span(_code, GPS_L5Q_CODE_LENGTH_CHIPS), _prn - 1); + make_l5q(_code_span, _prn - 1); } for (int32_t i = 0; i < GPS_L5Q_CODE_LENGTH_CHIPS; i++) { - _dest[i] = std::complex(1.0 - 2.0 * _code[i], 0.0); + _dest[i] = std::complex(1.0 - 2.0 * _code_span[i], 0.0); } delete[] _code; @@ -273,15 +274,15 @@ void gps_l5q_code_gen_complex(gsl::span> _dest, uint32_t _pr void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn) { auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; - + gsl::span _code_span(_code, GPS_L5Q_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - make_l5q(gsl::span(_code, GPS_L5Q_CODE_LENGTH_CHIPS), _prn - 1); + make_l5q(_code_span, _prn - 1); } for (int32_t i = 0; i < GPS_L5Q_CODE_LENGTH_CHIPS; i++) { - _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + _dest[i] = 1.0 - 2.0 * static_cast(_code_span[i]); } delete[] _code; @@ -294,9 +295,10 @@ void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn) void gps_l5q_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; + gsl::span _code_span(_code, GPS_L5Q_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { - make_l5q(gsl::span(_code, GPS_L5Q_CODE_LENGTH_CHIPS), _prn - 1); + make_l5q(_code_span, _prn - 1); } int32_t _samplesPerCode, _codeValueIndex; @@ -323,11 +325,11 @@ void gps_l5q_code_gen_complex_sampled(gsl::span> _dest, uint if (i == _samplesPerCode - 1) { //--- Correct the last index (due to number rounding issues) ----------- - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeLength - 1], 0); + _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeLength - 1], 0); } else { - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0); // repeat the chip -> upsample + _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeValueIndex], 0); // repeat the chip -> upsample } } delete[] _code; From f90d52a1e2b36d51b91e63cf07cd5171691217df Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 13:57:00 +0200 Subject: [PATCH 09/64] Improve const correctness --- .../libs/galileo_e1_signal_processing.cc | 14 +++++++------- src/algorithms/libs/galileo_e1_signal_processing.h | 10 +++++----- .../libs/galileo_e5_signal_processing.cc | 4 ++-- src/algorithms/libs/galileo_e5_signal_processing.h | 4 ++-- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index e594662fb..bdeba713b 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -37,7 +37,7 @@ #include -void galileo_e1_code_gen_int(gsl::span _dest, std::array _Signal, int32_t _prn) +void galileo_e1_code_gen_int(gsl::span _dest, const std::array& _Signal, int32_t _prn) { std::string _galileo_signal = _Signal.data(); int32_t prn = _prn - 1; @@ -105,7 +105,7 @@ void galileo_e1_sinboc_61_gen_int(gsl::span _dest, gsl::span _pr } -void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, std::array _Signal, uint32_t _prn) +void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, const std::array& _Signal, uint32_t _prn) { std::string _galileo_signal = _Signal.data(); const auto _codeLength = static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS); @@ -119,7 +119,7 @@ void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, std::array _dest, gsl::span _prn, std::array _Signal) +void galileo_e1_gen_float(gsl::span _dest, gsl::span _prn, const std::array& _Signal) { std::string _galileo_signal = _Signal.data(); const uint32_t _codeLength = 12 * GALILEO_E1_B_CODE_LENGTH_CHIPS; @@ -153,7 +153,7 @@ void galileo_e1_gen_float(gsl::span _dest, gsl::span _prn, std::arra } -void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, +void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag) { @@ -231,7 +231,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array> _dest, std::array _Signal, +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag) { @@ -257,14 +257,14 @@ void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, s } -void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, +void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { galileo_e1_code_gen_float_sampled(_dest, _Signal, _cboc, _prn, _fs, _chip_shift, false); } -void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { galileo_e1_code_gen_complex_sampled(_dest, _Signal, _cboc, _prn, _fs, _chip_shift, false); diff --git a/src/algorithms/libs/galileo_e1_signal_processing.h b/src/algorithms/libs/galileo_e1_signal_processing.h index 2c9c03d1d..544655b40 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.h +++ b/src/algorithms/libs/galileo_e1_signal_processing.h @@ -47,14 +47,14 @@ namespace gsl = std; * \brief This function generates Galileo E1 code (can select E1B or E1C sinboc). * */ -void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, std::array _Signal, uint32_t _prn); +void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, const std::array& _Signal, uint32_t _prn); /*! * \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc * and the sample frequency _fs). * */ -void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _Signal, +void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag); @@ -63,7 +63,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array _dest, std::array _Signal, +void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); /*! @@ -71,14 +71,14 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, std::array> _dest, std::array _Signal, +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag); /*! * \brief galileo_e1_code_gen_complex_sampled without _secondary_flag for backward compatibility. */ -void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GALILEO_E1_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/galileo_e5_signal_processing.cc b/src/algorithms/libs/galileo_e5_signal_processing.cc index 5df87e416..5f80de3e1 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.cc +++ b/src/algorithms/libs/galileo_e5_signal_processing.cc @@ -37,7 +37,7 @@ #include -void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, std::array _Signal) +void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, const std::array& _Signal) { uint32_t prn = _prn - 1; uint32_t index = 0; @@ -100,7 +100,7 @@ void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, } -void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, std::array _Signal, +void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, uint32_t _prn, int32_t _fs, uint32_t _chip_shift) { uint32_t _samplesPerCode; diff --git a/src/algorithms/libs/galileo_e5_signal_processing.h b/src/algorithms/libs/galileo_e5_signal_processing.h index 49235bd32..90d6abc5e 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.h +++ b/src/algorithms/libs/galileo_e5_signal_processing.h @@ -49,14 +49,14 @@ namespace gsl = std; * \brief Generates Galileo E5a code at 1 sample/chip * bool _pilot generates E5aQ code if true and E5aI (data signal) if false. */ -void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, std::array _Signal); +void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, const std::array& _Signal); /*! * \brief Generates Galileo E5a complex code, shifted to the desired chip and sampled at a frequency fs * bool _pilot generates E5aQ code if true and E5aI (data signal) if false. */ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, - std::array _Signal, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); + const std::array& _Signal, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GALILEO_E5_SIGNAL_PROCESSING_H_ */ From c37b3e00b5a2e2f5560a465437b5cc9c8484337e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 14:22:15 +0200 Subject: [PATCH 10/64] Use uppercase literal suffix --- src/algorithms/PVT/libs/rtcm.cc | 6 +++--- .../libs/beidou_b1i_signal_processing.cc | 2 +- .../libs/beidou_b3i_signal_processing.cc | 2 +- .../libs/galileo_e1_signal_processing.cc | 4 ++-- src/algorithms/libs/gps_sdr_signal_processing.cc | 2 +- src/algorithms/libs/rtklib/rtklib_ionex.cc | 2 +- src/algorithms/libs/rtklib/rtklib_preceph.cc | 8 ++++---- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 16 ++++++++-------- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 4 ++-- src/algorithms/libs/rtklib/rtklib_sbas.cc | 6 +++--- .../gnuradio_blocks/rtl_tcp_signal_source_c.cc | 2 +- .../gps_l2c_telemetry_decoder_gs.cc | 2 +- .../gps_l5_telemetry_decoder_gs.cc | 2 +- .../telemetry_decoder/libs/libswiftcnav/bits.c | 10 +++++----- .../libs/libswiftcnav/cnav_msg.c | 6 +++--- .../telemetry_decoder/libs/libswiftcnav/edc.c | 12 ++++++------ .../system_parameters/galileo_fnav_message.cc | 2 +- .../galileo_navigation_message.cc | 2 +- 18 files changed, 45 insertions(+), 45 deletions(-) diff --git a/src/algorithms/PVT/libs/rtcm.cc b/src/algorithms/PVT/libs/rtcm.cc index deb7a997b..e514d733a 100644 --- a/src/algorithms/PVT/libs/rtcm.cc +++ b/src/algorithms/PVT/libs/rtcm.cc @@ -140,7 +140,7 @@ bool Rtcm::is_server_running() const std::string Rtcm::add_CRC(const std::string& message_without_crc) const { // ****** Computes Qualcomm CRC-24Q ****** - boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> CRC_RTCM; + boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false> CRC_RTCM; // 1) Converts the string to a vector of uint8_t: boost::dynamic_bitset frame_bits(message_without_crc); std::vector bytes; @@ -159,7 +159,7 @@ std::string Rtcm::add_CRC(const std::string& message_without_crc) const bool Rtcm::check_CRC(const std::string& message) const { - boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> CRC_RTCM_CHECK; + boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false> CRC_RTCM_CHECK; // Convert message to binary std::string message_bin = Rtcm::binary_data_to_bin(message); // Check CRC @@ -5330,7 +5330,7 @@ int32_t Rtcm::set_DF398(const Gnss_Synchro& gnss_synchro) } else { - rr_mod_ms = static_cast(std::floor(rough_range_m / meters_to_miliseconds / TWO_N10) + 0.5) & 0x3FFu; + rr_mod_ms = static_cast(std::floor(rough_range_m / meters_to_miliseconds / TWO_N10) + 0.5) & 0x3FFU; } DF398 = std::bitset<10>(rr_mod_ms); return 0; diff --git a/src/algorithms/libs/beidou_b1i_signal_processing.cc b/src/algorithms/libs/beidou_b1i_signal_processing.cc index 570c1e03a..5b848e6e7 100644 --- a/src/algorithms/libs/beidou_b1i_signal_processing.cc +++ b/src/algorithms/libs/beidou_b1i_signal_processing.cc @@ -135,7 +135,7 @@ void beidou_b1i_code_gen_complex(gsl::span> _dest, int32_t _ for (uint32_t ii = 0; ii < _code_length; ++ii) { - _dest[ii] = std::complex(static_cast(b1i_code_int[ii]), 0.0f); + _dest[ii] = std::complex(static_cast(b1i_code_int[ii]), 0.0F); } } diff --git a/src/algorithms/libs/beidou_b3i_signal_processing.cc b/src/algorithms/libs/beidou_b3i_signal_processing.cc index 2c36fd64d..b7f0cd2a5 100644 --- a/src/algorithms/libs/beidou_b3i_signal_processing.cc +++ b/src/algorithms/libs/beidou_b3i_signal_processing.cc @@ -193,7 +193,7 @@ void beidou_b3i_code_gen_complex(gsl::span> _dest, signed in for (unsigned int ii = 0; ii < _code_length; ++ii) { - _dest[ii] = std::complex(static_cast(b3i_code_int[ii]), 0.0f); + _dest[ii] = std::complex(static_cast(b3i_code_int[ii]), 0.0F); } } diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index bdeba713b..f0294e834 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -211,7 +211,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< { for (unsigned k = 0; k < _samplesPerCode; k++) { - _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); + _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0F : -1.0F); } } @@ -251,7 +251,7 @@ void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, c for (uint32_t ii = 0; ii < _samplesPerCode; ++ii) { - _dest[ii] = std::complex(real_code_span[ii], 0.0f); + _dest[ii] = std::complex(real_code_span[ii], 0.0F); } volk_gnsssdr_free(real_code); } diff --git a/src/algorithms/libs/gps_sdr_signal_processing.cc b/src/algorithms/libs/gps_sdr_signal_processing.cc index 3375e3b40..4f7411dc6 100644 --- a/src/algorithms/libs/gps_sdr_signal_processing.cc +++ b/src/algorithms/libs/gps_sdr_signal_processing.cc @@ -139,7 +139,7 @@ void gps_l1_ca_code_gen_complex(gsl::span> _dest, int32_t _p for (uint32_t ii = 0; ii < _code_length; ++ii) { - _dest[ii] = std::complex(static_cast(ca_code_int[ii]), 0.0f); + _dest[ii] = std::complex(static_cast(ca_code_int[ii]), 0.0F); } } diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc index 8b8d39ade..d07740dc7 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.cc +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -149,7 +149,7 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts, for (i = 0; i < n; i++) { p->data[i] = 0.0; - p->rms[i] = 0.0f; + p->rms[i] = 0.0F; } nav->nt++; return p; diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc index 61217a8ae..c1b5c23e6 100644 --- a/src/algorithms/libs/rtklib/rtklib_preceph.cc +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -209,14 +209,14 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, co for (j = 0; j < 4; j++) { peph.pos[i][j] = 0.0; - peph.std[i][j] = 0.0f; + peph.std[i][j] = 0.0F; peph.vel[i][j] = 0.0; - peph.vst[i][j] = 0.0f; + peph.vst[i][j] = 0.0F; } for (j = 0; j < 3; j++) { - peph.cov[i][j] = 0.0f; - peph.vco[i][j] = 0.0f; + peph.cov[i][j] = 0.0F; + peph.vco[i][j] = 0.0F; } } for (i = pred_o = pred_c = v = 0; i < n && fgets(buff, sizeof(buff), fp); i++) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 8f9e331fc..fc60a4221 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -778,7 +778,7 @@ unsigned int getbitu(const unsigned char *buff, int pos, int len) int i; for (i = pos; i < pos + len; i++) { - bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1u); + bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1U); } return bits; } @@ -787,11 +787,11 @@ unsigned int getbitu(const unsigned char *buff, int pos, int len) int getbits(const unsigned char *buff, int pos, int len) { unsigned int bits = getbitu(buff, pos, len); - if (len <= 0 || 32 <= len || !(bits & (1u << (len - 1)))) + if (len <= 0 || 32 <= len || !(bits & (1U << (len - 1)))) { return static_cast(bits); } - return static_cast(bits | (~0u << len)); /* extend sign */ + return static_cast(bits | (~0U << len)); /* extend sign */ } @@ -805,7 +805,7 @@ int getbits(const unsigned char *buff, int pos, int len) *-----------------------------------------------------------------------------*/ void setbitu(unsigned char *buff, int pos, int len, unsigned int data) { - unsigned int mask = 1u << (len - 1); + unsigned int mask = 1U << (len - 1); int i; if (len <= 0 || 32 < len) { @@ -815,11 +815,11 @@ void setbitu(unsigned char *buff, int pos, int len, unsigned int data) { if (data & mask) { - buff[i / 8] |= 1u << (7 - i % 8); + buff[i / 8] |= 1U << (7 - i % 8); } else { - buff[i / 8] &= ~(1u << (7 - i % 8)); + buff[i / 8] &= ~(1U << (7 - i % 8)); } } } @@ -2080,10 +2080,10 @@ unsigned int tickget(void) /* linux kernel > 2.6.28 */ if (!clock_gettime(CLOCK_MONOTONIC_RAW, &tp)) { - return tp.tv_sec * 1000u + tp.tv_nsec / 1000000u; + return tp.tv_sec * 1000U + tp.tv_nsec / 1000000U; } gettimeofday(&tv, nullptr); - return tv.tv_sec * 1000u + tv.tv_usec / 1000u; + return tv.tv_sec * 1000U + tv.tv_usec / 1000U; #else gettimeofday(&tv, NULL); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index e59327680..65e3f6905 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -2075,10 +2075,10 @@ int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa) trace(4, "N(2)="); tracemat(4, b + nb, 1, nb, 10, 3); - rtk->sol.ratio = s[0] > 0 ? static_cast(s[1] / s[0]) : 0.0f; + rtk->sol.ratio = s[0] > 0 ? static_cast(s[1] / s[0]) : 0.0F; if (rtk->sol.ratio > 999.9) { - rtk->sol.ratio = 999.9f; + rtk->sol.ratio = 999.9F; } /* validation by popular ratio-test */ diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc index 943b271f0..8b5c7c774 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.cc +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -188,7 +188,7 @@ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) t0 = sbssat->sat[j].fcorr.t0; prc = sbssat->sat[j].fcorr.prc; sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i * 12, 12) * 0.125f; + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i * 12, 12) * 0.125F; sbssat->sat[j].fcorr.udre = udre + 1; dt = timediff(sbssat->sat[j].fcorr.t0, t0); if (t0.time == 0 || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0) @@ -468,7 +468,7 @@ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) udre = getbitu(msg->msg, 86 + 4 * i, 4); sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i * 12, 12) * 0.125f; + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i * 12, 12) * 0.125F; sbssat->sat[j].fcorr.udre = udre + 1; sbssat->sat[j].fcorr.iodf = iodf; } @@ -509,7 +509,7 @@ int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) delay = getbitu(msg->msg, 22 + i * 13, 9); sbsion[band].igp[j].t0 = gpst2time(msg->week, msg->tow); - sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay * 0.125f; + sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0F : delay * 0.125F; sbsion[band].igp[j].give = give + 1; if (sbsion[band].igp[j].give >= 16) diff --git a/src/algorithms/signal_source/gnuradio_blocks/rtl_tcp_signal_source_c.cc b/src/algorithms/signal_source/gnuradio_blocks/rtl_tcp_signal_source_c.cc index 188da4bdc..bbc5b57d6 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/rtl_tcp_signal_source_c.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/rtl_tcp_signal_source_c.cc @@ -76,7 +76,7 @@ rtl_tcp_signal_source_c::rtl_tcp_signal_source_c(const std::string &address, // 1. Setup lookup table for (unsigned i = 0; i < 0xff; i++) { - lookup_[i] = (static_cast(i & 0xff) - 127.4f) * (1.0f / 128.0f); + lookup_[i] = (static_cast(i & 0xff) - 127.4F) * (1.0F / 128.0F); } // 2. Set socket options diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc index ebca21346..44cd5cd40 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc @@ -190,7 +190,7 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder for (uint32_t i = 0; i < GPS_L2_CNAV_DATA_PAGE_BITS; i++) { - raw_bits[GPS_L2_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1u); + raw_bits[GPS_L2_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1U); } d_CNAV_Message.decode_page(raw_bits); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index b5329b8d2..ea8a87e44 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -241,7 +241,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u // Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder for (uint32_t i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS; i++) { - raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1u); + raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1U); } d_CNAV_Message.decode_page(raw_bits); diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c index 1cbe9eb12..3544aad03 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c @@ -75,7 +75,7 @@ uint32_t getbitu(const uint8_t *buff, uint32_t pos, uint8_t len) for (i = pos; i < pos + len; i++) { bits = (bits << 1) + - ((buff[i / 8] >> (7 - i % 8)) & 1u); + ((buff[i / 8] >> (7 - i % 8)) & 1U); } return bits; @@ -99,7 +99,7 @@ int32_t getbits(const uint8_t *buff, uint32_t pos, uint8_t len) /* Sign extend, taken from: * http://graphics.stanford.edu/~seander/bithacks.html#VariableSignExtend */ - int32_t m = 1u << (len - 1); + int32_t m = 1U << (len - 1); return (bits ^ m) - m; } @@ -114,7 +114,7 @@ int32_t getbits(const uint8_t *buff, uint32_t pos, uint8_t len) */ void setbitu(uint8_t *buff, uint32_t pos, uint32_t len, uint32_t data) { - uint32_t mask = 1u << (len - 1); + uint32_t mask = 1U << (len - 1); if (len <= 0 || 32 < len) { @@ -125,11 +125,11 @@ void setbitu(uint8_t *buff, uint32_t pos, uint32_t len, uint32_t data) { if (data & mask) { - buff[i / 8] |= 1u << (7 - i % 8); + buff[i / 8] |= 1U << (7 - i % 8); } else { - buff[i / 8] &= ~(1u << (7 - i % 8)); + buff[i / 8] &= ~(1U << (7 - i % 8)); } } } diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index 1676fc0cd..260237d40 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -56,9 +56,9 @@ */ /** GPS L2C preamble */ -const uint32_t GPS_CNAV_PREAMBLE1 = 0x8Bu; /* (0b10001011u) */ +const uint32_t GPS_CNAV_PREAMBLE1 = 0x8BU; /* (0b10001011u) */ /** Inverted GPS L2C preamble */ -const uint32_t GPS_CNAV_PREAMBLE2 = 0x74u; /* (0b01110100u) */ +const uint32_t GPS_CNAV_PREAMBLE2 = 0x74U; /* (0b01110100u) */ /** GPS L2C preamble length in bits */ #define GPS_CNAV_PREAMBLE_LENGTH (8) /** GPS L2C CNAV message length in bits */ @@ -307,7 +307,7 @@ static void _cnav_msg_invert(cnav_v27_part_t *part) size_t i = 0; for (i = 0; i < sizeof(part->decoded); i++) { - part->decoded[i] ^= 0xFFu; + part->decoded[i] ^= 0xFFU; } } diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c index 6e3e06c6f..7bb1e644b 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c @@ -125,18 +125,18 @@ uint32_t crc24q_bits(uint32_t crc, const uint8_t *buf, uint32_t n_bits, bool inv acc = (acc << 8) | *buf++; if (invert) { - acc ^= 0xFFu; + acc ^= 0xFFU; } - b = (acc >> shift) & 0xFFu; - crc = ((crc << 8) & 0xFFFFFFu) ^ CRC24QTAB[((crc >> 16) ^ b) & 0xFFu]; + b = (acc >> shift) & 0xFFU; + crc = ((crc << 8) & 0xFFFFFFU) ^ CRC24QTAB[((crc >> 16) ^ b) & 0xFFU]; } acc = (acc << 8) | *buf; if (invert) { - acc ^= 0xFFu; + acc ^= 0xFFU; } - b = (acc >> shift) & 0xFFu; - crc = ((crc << 8) & 0xFFFFFFu) ^ CRC24QTAB[((crc >> 16) ^ b) & 0xFFu]; + b = (acc >> shift) & 0xFFU; + crc = ((crc << 8) & 0xFFFFFFU) ^ CRC24QTAB[((crc >> 16) ^ b) & 0xFFU]; return crc; } diff --git a/src/core/system_parameters/galileo_fnav_message.cc b/src/core/system_parameters/galileo_fnav_message.cc index 9dc6eb49e..3807fc143 100644 --- a/src/core/system_parameters/galileo_fnav_message.cc +++ b/src/core/system_parameters/galileo_fnav_message.cc @@ -42,7 +42,7 @@ #include // for string, operator<< #include // for back_insert_iterator -using CRC_Galileo_FNAV_type = boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false>; +using CRC_Galileo_FNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; void Galileo_Fnav_Message::reset() { diff --git a/src/core/system_parameters/galileo_navigation_message.cc b/src/core/system_parameters/galileo_navigation_message.cc index 62fe2d243..cb5f55b02 100644 --- a/src/core/system_parameters/galileo_navigation_message.cc +++ b/src/core/system_parameters/galileo_navigation_message.cc @@ -38,7 +38,7 @@ #include // for operator<< -using CRC_Galileo_INAV_type = boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false>; +using CRC_Galileo_INAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; void Galileo_Navigation_Message::reset() From 81c79ad0071a706b0c66018c40ec2c00b3c8d2af Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 16:49:53 +0200 Subject: [PATCH 11/64] Fix termination --- src/algorithms/libs/galileo_e1_signal_processing.cc | 9 +++++---- src/algorithms/libs/galileo_e1_signal_processing.h | 10 +++++----- src/algorithms/libs/galileo_e5_signal_processing.cc | 2 +- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index f0294e834..599e8df7e 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -176,15 +176,16 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< _codeLength = _samplesPerChip * GALILEO_E1_B_CODE_LENGTH_CHIPS; _signal_E1 = new float[_codeLength]; gsl::span _signal_E1_span(_signal_E1, _codeLength); + if (_cboc == true) { - galileo_e1_gen_float(gsl::span(_signal_E1, _codeLength), gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)), _Signal); // generate cboc 12 samples per chip + galileo_e1_gen_float(_signal_E1_span, gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)), _Signal); // generate cboc 12 samples per chip } else { auto* _signal_E1_int = static_cast(volk_gnsssdr_malloc(_codeLength * sizeof(int32_t), volk_gnsssdr_get_alignment())); gsl::span _signal_E1_int_span(_signal_E1_int, _codeLength); - galileo_e1_sinboc_11_gen_int(gsl::span(_signal_E1_int, _codeLength), gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS))); // generate sinboc(1,1) 2 samples per chip + galileo_e1_sinboc_11_gen_int(_signal_E1_int_span, gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS))); // generate sinboc(1,1) 2 samples per chip for (uint32_t ii = 0; ii < _codeLength; ++ii) { @@ -211,7 +212,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< { for (unsigned k = 0; k < _samplesPerCode; k++) { - _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0F : -1.0F); + _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); } } @@ -223,7 +224,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< for (uint32_t i = 0; i < _samplesPerCode; i++) { - _dest[(i + delay) % _samplesPerCode] = _signal_E1_span[i]; + _dest[(i + delay) % _samplesPerCode] = _signal_E1[i]; } delete[] _signal_E1; diff --git a/src/algorithms/libs/galileo_e1_signal_processing.h b/src/algorithms/libs/galileo_e1_signal_processing.h index 544655b40..161d104cb 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.h +++ b/src/algorithms/libs/galileo_e1_signal_processing.h @@ -47,14 +47,14 @@ namespace gsl = std; * \brief This function generates Galileo E1 code (can select E1B or E1C sinboc). * */ -void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, const std::array& _Signal, uint32_t _prn); +void galileo_e1_code_gen_sinboc11_float(gsl::span _dest, const std::array& _Signal, uint32_t _prn); /*! * \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc * and the sample frequency _fs). * */ -void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, +void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag); @@ -63,7 +63,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< * and the sample frequency _fs). * */ -void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, +void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); /*! @@ -71,14 +71,14 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< * and the sample frequency _fs). * */ -void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift, bool _secondary_flag); /*! * \brief galileo_e1_code_gen_complex_sampled without _secondary_flag for backward compatibility. */ -void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, +void galileo_e1_code_gen_complex_sampled(gsl::span> _dest, const std::array& _Signal, bool _cboc, uint32_t _prn, int32_t _fs, uint32_t _chip_shift); #endif /* GNSS_SDR_GALILEO_E1_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/libs/galileo_e5_signal_processing.cc b/src/algorithms/libs/galileo_e5_signal_processing.cc index 5f80de3e1..5aa72b35c 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.cc +++ b/src/algorithms/libs/galileo_e5_signal_processing.cc @@ -129,7 +129,7 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, for (uint32_t i = 0; i < _samplesPerCode; i++) { - _dest[(i + delay) % _samplesPerCode] = _code_span[i]; + _dest[(i + delay) % _samplesPerCode] = _code[i]; } if (_fs != _codeFreqBasis) { From 0b1683fa1ed8a86cb0ca1c7e15f6890b555454be Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 17:29:51 +0200 Subject: [PATCH 12/64] Avoid pointer arithmetics --- .../libs/galileo_e1_signal_processing.cc | 17 +++++++++++++---- .../libs/galileo_e5_signal_processing.cc | 9 +++++++-- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index 599e8df7e..085fe8e78 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -203,7 +203,12 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< delete[] _signal_E1; _signal_E1 = _resampled_signal; } - + uint32_t size_signal_E1 = _codeLength; + if (_fs != _samplesPerChip * _codeFreqBasis) + { + size_signal_E1 = _samplesPerCode; + } + gsl::span _signal_E1_span_aux(_signal_E1, size_signal_E1); if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2 && _secondary_flag) { auto* _signal_E1C_secondary = new float[static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode]; @@ -212,7 +217,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< { for (unsigned k = 0; k < _samplesPerCode; k++) { - _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); + _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span_aux[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); } } @@ -221,10 +226,14 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< delete[] _signal_E1; _signal_E1 = _signal_E1C_secondary_span.data(); } - + if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2 && _secondary_flag) + { + size_signal_E1 = static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode; + } + gsl::span _signal_E1_span_aux2(_signal_E1, size_signal_E1); for (uint32_t i = 0; i < _samplesPerCode; i++) { - _dest[(i + delay) % _samplesPerCode] = _signal_E1[i]; + _dest[(i + delay) % _samplesPerCode] = _signal_E1_span_aux2[i]; } delete[] _signal_E1; diff --git a/src/algorithms/libs/galileo_e5_signal_processing.cc b/src/algorithms/libs/galileo_e5_signal_processing.cc index 5aa72b35c..38a6e79e3 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.cc +++ b/src/algorithms/libs/galileo_e5_signal_processing.cc @@ -126,10 +126,15 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, delete[] _code; _code = _resampled_signal; } - + uint32_t size_code = _codeLength; + if (_fs != _codeFreqBasis) + { + size_code = _samplesPerCode; + } + gsl::span> _code_span_aux(_code, size_code); for (uint32_t i = 0; i < _samplesPerCode; i++) { - _dest[(i + delay) % _samplesPerCode] = _code[i]; + _dest[(i + delay) % _samplesPerCode] = _code_span_aux[i]; } if (_fs != _codeFreqBasis) { From dd3b2f11dbaccf9b513935285d6e88da355dd45b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 21:13:14 +0200 Subject: [PATCH 13/64] Fix wrong loop --- .../adapters/gps_l1_ca_pcps_quicksync_acquisition.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc index d28420e7c..2867b53a0 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc @@ -242,7 +242,7 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_local_code() gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); gsl::span code_span(code_, vector_length_); - for (unsigned int i = 0; i < sampled_ms_; i++) + for (unsigned int i = 0; i < (sampled_ms_ / folding_factor_); i++) { std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); } From b6e9ba58773ceb0c8ce1998d5b70931fab194ff5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 22:04:03 +0200 Subject: [PATCH 14/64] Improve const correctness --- src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc | 2 +- src/algorithms/PVT/libs/monitor_pvt_udp_sink.h | 2 +- src/algorithms/PVT/libs/nmea_printer.cc | 2 +- src/algorithms/PVT/libs/nmea_printer.h | 2 +- src/core/libs/INIReader.cc | 2 +- src/core/libs/INIReader.h | 2 +- src/core/monitor/gnss_synchro_udp_sink.cc | 2 +- src/core/monitor/gnss_synchro_udp_sink.h | 2 +- src/core/receiver/gnss_flowgraph.cc | 2 +- src/core/receiver/gnss_flowgraph.h | 2 +- src/core/receiver/in_memory_configuration.cc | 2 +- src/core/receiver/in_memory_configuration.h | 2 +- 12 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc index baa85692d..0e962207b 100644 --- a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc @@ -35,7 +35,7 @@ #include -Monitor_Pvt_Udp_Sink::Monitor_Pvt_Udp_Sink(std::vector addresses, const uint16_t& port, bool protobuf_enabled) : socket{io_context} +Monitor_Pvt_Udp_Sink::Monitor_Pvt_Udp_Sink(const std::vector& addresses, const uint16_t& port, bool protobuf_enabled) : socket{io_context} { for (const auto& address : addresses) { diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h index c8c363942..85a612358 100644 --- a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h @@ -45,7 +45,7 @@ using b_io_context = boost::asio::io_service; class Monitor_Pvt_Udp_Sink { public: - Monitor_Pvt_Udp_Sink(std::vector addresses, const uint16_t &port, bool protobuf_enabled); + Monitor_Pvt_Udp_Sink(const std::vector& addresses, const uint16_t &port, bool protobuf_enabled); bool write_monitor_pvt(const Monitor_Pvt &monitor_pvt); private: diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index ecc92b914..913a23e39 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -283,7 +283,7 @@ bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr& pvt_dat } -char Nmea_Printer::checkSum(std::string sentence) +char Nmea_Printer::checkSum(const std::string& sentence) { char check = 0; // iterate over the string, XOR each byte with the total sum: diff --git a/src/algorithms/PVT/libs/nmea_printer.h b/src/algorithms/PVT/libs/nmea_printer.h index f821ad94b..d9822c6c5 100644 --- a/src/algorithms/PVT/libs/nmea_printer.h +++ b/src/algorithms/PVT/libs/nmea_printer.h @@ -83,7 +83,7 @@ private: std::string get_UTC_NMEA_time(boost::posix_time::ptime d_position_UTC_time); std::string longitude_to_hm(double longitude); std::string latitude_to_hm(double lat); - char checkSum(std::string sentence); + char checkSum(const std::string& sentence); bool print_avg_pos; bool d_flag_nmea_output_file; }; diff --git a/src/core/libs/INIReader.cc b/src/core/libs/INIReader.cc index 63c076b82..454986615 100644 --- a/src/core/libs/INIReader.cc +++ b/src/core/libs/INIReader.cc @@ -64,7 +64,7 @@ int INIReader::ParseError() } -std::string INIReader::Get(const std::string& section, const std::string& name, std::string default_value) +std::string INIReader::Get(const std::string& section, const std::string& name, const std::string& default_value) { std::string key = MakeKey(section, name); return _values.count(key) ? _values[key] : default_value; diff --git a/src/core/libs/INIReader.h b/src/core/libs/INIReader.h index bfe168bd9..1a9f0f3e5 100644 --- a/src/core/libs/INIReader.h +++ b/src/core/libs/INIReader.h @@ -67,7 +67,7 @@ public: //! Get a string value from INI file, returning default_value if not found. std::string Get(const std::string& section, const std::string& name, - std::string default_value); + const std::string& default_value); //! Get an integer (long) value from INI file, returning default_value if not found. int64_t GetInteger(const std::string& section, const std::string& name, int64_t default_value); diff --git a/src/core/monitor/gnss_synchro_udp_sink.cc b/src/core/monitor/gnss_synchro_udp_sink.cc index 5793358ac..5a412a593 100644 --- a/src/core/monitor/gnss_synchro_udp_sink.cc +++ b/src/core/monitor/gnss_synchro_udp_sink.cc @@ -35,7 +35,7 @@ #include #include -Gnss_Synchro_Udp_Sink::Gnss_Synchro_Udp_Sink(std::vector addresses, const uint16_t& port, bool enable_protobuf) : socket{io_context} +Gnss_Synchro_Udp_Sink::Gnss_Synchro_Udp_Sink(const std::vector& addresses, const uint16_t& port, bool enable_protobuf) : socket{io_context} { use_protobuf = enable_protobuf; if (enable_protobuf) diff --git a/src/core/monitor/gnss_synchro_udp_sink.h b/src/core/monitor/gnss_synchro_udp_sink.h index 0cf4cc5cd..52c786885 100644 --- a/src/core/monitor/gnss_synchro_udp_sink.h +++ b/src/core/monitor/gnss_synchro_udp_sink.h @@ -53,7 +53,7 @@ using b_io_context = boost::asio::io_service; class Gnss_Synchro_Udp_Sink { public: - Gnss_Synchro_Udp_Sink(std::vector addresses, const uint16_t& port, bool enable_protobuf); + Gnss_Synchro_Udp_Sink(const std::vector& addresses, const uint16_t& port, bool enable_protobuf); bool write_gnss_synchro(const std::vector& stocks); private: diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 0611a4e26..1de87bc17 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1511,7 +1511,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } -void GNSSFlowgraph::priorize_satellites(std::vector> visible_satellites) +void GNSSFlowgraph::priorize_satellites(const std::vector>& visible_satellites) { size_t old_size; Gnss_Signal gs; diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 32e0ea2e3..7552c46a7 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -142,7 +142,7 @@ public: /*! * \brief Priorize visible satellites in the specified vector */ - void priorize_satellites(std::vector> visible_satellites); + void priorize_satellites(const std::vector>& visible_satellites); private: void init(); // Populates the SV PRN list available for acquisition and tracking diff --git a/src/core/receiver/in_memory_configuration.cc b/src/core/receiver/in_memory_configuration.cc index 4801d2eb2..c3c093104 100644 --- a/src/core/receiver/in_memory_configuration.cc +++ b/src/core/receiver/in_memory_configuration.cc @@ -128,7 +128,7 @@ void InMemoryConfiguration::set_property(std::string property_name, std::string } -void InMemoryConfiguration::supersede_property(std::string property_name, std::string value) +void InMemoryConfiguration::supersede_property(const std::string& property_name, const std::string& value) { properties_.erase(property_name); properties_.insert(std::make_pair(property_name, value)); diff --git a/src/core/receiver/in_memory_configuration.h b/src/core/receiver/in_memory_configuration.h index 698071a36..2201c0d61 100644 --- a/src/core/receiver/in_memory_configuration.h +++ b/src/core/receiver/in_memory_configuration.h @@ -66,7 +66,7 @@ public: float property(std::string property_name, float default_value); double property(std::string property_name, double default_value); void set_property(std::string property_name, std::string value); - void supersede_property(std::string property_name, std::string value); + void supersede_property(const std::string& property_name, const std::string& value); bool is_present(const std::string& property_name); private: From 72648884876c124bd35159e3cfbdbb6d70464a56 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 22:32:29 +0200 Subject: [PATCH 15/64] Add explicit keyword See https://google.github.io/styleguide/cppguide.html#Explicit_Constructors --- .../signal-processing-blocks/tracking/cubature_filter_test.cc | 4 ++-- .../tracking/unscented_filter_test.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc index 56e18df38..0833b12be 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc @@ -39,7 +39,7 @@ class TransitionModel : public ModelFunction { public: - TransitionModel(const arma::mat& kf_F) { coeff_mat = kf_F; }; + explicit TransitionModel(const arma::mat& kf_F) { coeff_mat = kf_F; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; private: @@ -49,7 +49,7 @@ private: class MeasurementModel : public ModelFunction { public: - MeasurementModel(const arma::mat& kf_H) { coeff_mat = kf_H; }; + explicit MeasurementModel(const arma::mat& kf_H) { coeff_mat = kf_H; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; private: diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc index eaef07845..bc9e5315d 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc @@ -39,7 +39,7 @@ class TransitionModelUKF : public ModelFunction { public: - TransitionModelUKF(const arma::mat& kf_F) { coeff_mat = kf_F; }; + explicit TransitionModelUKF(const arma::mat& kf_F) { coeff_mat = kf_F; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; private: @@ -49,7 +49,7 @@ private: class MeasurementModelUKF : public ModelFunction { public: - MeasurementModelUKF(const arma::mat& kf_H) { coeff_mat = kf_H; }; + explicit MeasurementModelUKF(const arma::mat& kf_H) { coeff_mat = kf_H; }; virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; private: From f821caea4a000398dfd6938802f6eb647d1376ed Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 22:47:21 +0200 Subject: [PATCH 16/64] Code cleaning --- .../libs/beidou_b3i_signal_processing.cc | 4 +- .../gnuradio_blocks/hybrid_observables_gs.cc | 48 +++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/algorithms/libs/beidou_b3i_signal_processing.cc b/src/algorithms/libs/beidou_b3i_signal_processing.cc index b7f0cd2a5..ed12c4b30 100644 --- a/src/algorithms/libs/beidou_b3i_signal_processing.cc +++ b/src/algorithms/libs/beidou_b3i_signal_processing.cc @@ -113,7 +113,9 @@ void beidou_b3i_code_gen_int(gsl::span _dest, signed int _prn, unsigned int // A simple error check if ((prn_idx < 0) || (prn_idx > 63)) - return; + { + return; + } // Assign shifted G2 register based on prn number G2_register = G2_register_shifted[prn_idx]; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 935f01f5c..cceb030e6 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -424,15 +424,15 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const int32_t t2_idx; if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter) { - //std::cout << "S1= " << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter - // << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter << std::endl; + // std::cout << "S1= " << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter + // << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter << std::endl; t1_idx = nearest_element; t2_idx = neighbor_element; } else { - //std::cout << "inv S1= " << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter - // << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter << std::endl; + // std::cout << "inv S1= " << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter + // << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter << std::endl; t1_idx = neighbor_element; t2_idx = nearest_element; } @@ -441,7 +441,6 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const interpolated_obs = d_gnss_synchro_history->at(ch, nearest_element); // 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1) - double T_rx_s = static_cast(rx_clock) / static_cast(interpolated_obs.fs); double time_factor = (T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time) / @@ -460,18 +459,18 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const } else { - //TOW rollover situation + // TOW rollover situation interpolated_obs.interp_TOW_ms = static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms + 604800000) - static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor; } - // LOG(INFO) << "Channel " << ch << " int idx: " << t1_idx << " TOW Int: " << interpolated_obs.interp_TOW_ms - // << " TOW p1 : " << d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms - // << " TOW p2: " - // << d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms - // << " t2-t1: " - // << d_gnss_synchro_history->at(ch, t2_idx).RX_time - d_gnss_synchro_history->at(ch, t1_idx).RX_time - // << " trx - t1: " - // << T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time; + // LOG(INFO) << "Channel " << ch << " int idx: " << t1_idx << " TOW Int: " << interpolated_obs.interp_TOW_ms + // << " TOW p1 : " << d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms + // << " TOW p2: " + // << d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms + // << " t2-t1: " + // << d_gnss_synchro_history->at(ch, t2_idx).RX_time - d_gnss_synchro_history->at(ch, t1_idx).RX_time + // << " trx - t1: " + // << T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time; // // std::cout << "Rx samplestamp: " << T_rx_s << " Channel " << ch << " interp buff idx " << nearest_element @@ -501,10 +500,10 @@ void hybrid_observables_gs::forecast(int noutput_items __attribute__((unused)), void hybrid_observables_gs::update_TOW(const std::vector &data) { - //1. Set the TOW using the minimum TOW in the observables. - // this will be the receiver time. - //2. If the TOW is set, it must be incremented by the desired receiver time step. - // the time step must match the observables timer block (connected to the las input channel) + // 1. Set the TOW using the minimum TOW in the observables. + // this will be the receiver time. + // 2. If the TOW is set, it must be incremented by the desired receiver time step. + // the time step must match the observables timer block (connected to the las input channel) std::vector::const_iterator it; if (!T_rx_TOW_set) { @@ -526,7 +525,7 @@ void hybrid_observables_gs::update_TOW(const std::vector &data) } else { - T_rx_TOW_ms += T_rx_step_ms; //the tow time step increment must match the ref time channel step + T_rx_TOW_ms += T_rx_step_ms; // the tow time step increment must match the ref time channel step if (T_rx_TOW_ms >= 604800000) { DLOG(INFO) << "TOW RX TIME rollover!"; @@ -538,8 +537,8 @@ void hybrid_observables_gs::update_TOW(const std::vector &data) void hybrid_observables_gs::compute_pranges(std::vector &data) { - // std::cout.precision(17); - // std::cout << " T_rx_TOW_ms: " << static_cast(T_rx_TOW_ms) << std::endl; + // std::cout.precision(17); + // std::cout << " T_rx_TOW_ms: " << static_cast(T_rx_TOW_ms) << std::endl; std::vector::iterator it; double current_T_rx_TOW_ms = (static_cast(T_rx_TOW_ms - T_rx_remnant_to_20ms)); double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0; @@ -548,7 +547,7 @@ void hybrid_observables_gs::compute_pranges(std::vector &data) if (it->Flag_valid_word) { double traveltime_ms = current_T_rx_TOW_ms - it->interp_TOW_ms; - if (fabs(traveltime_ms) > 302400) //check TOW roll over + if (fabs(traveltime_ms) > 302400) // check TOW roll over { traveltime_ms = 604800000.0 + current_T_rx_TOW_ms - it->interp_TOW_ms; } @@ -556,9 +555,8 @@ void hybrid_observables_gs::compute_pranges(std::vector &data) it->Pseudorange_m = traveltime_ms * SPEED_OF_LIGHT_MS; it->Flag_valid_pseudorange = true; // debug code - // - // std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl; - // std::cout << "[" << it->Channel_ID << "] Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl; + // std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl; + // std::cout << "[" << it->Channel_ID << "] Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl; } else { From d75abe6758a637e995e96f996b863eca0e6caee9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 29 Jun 2019 22:48:00 +0200 Subject: [PATCH 17/64] Add inline keyword --- src/tests/common-files/gnuplot_i.h | 82 +++++++++++++++--------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/src/tests/common-files/gnuplot_i.h b/src/tests/common-files/gnuplot_i.h index 2f5d7d979..4e9a4568f 100644 --- a/src/tests/common-files/gnuplot_i.h +++ b/src/tests/common-files/gnuplot_i.h @@ -1023,7 +1023,7 @@ Gnuplot &Gnuplot::plot_xyz(const X &x, // define static member function: set Gnuplot path manual // for windows: path with slash '/' not backslash '\' // -bool Gnuplot::set_GNUPlotPath(const std::string &path) +inline bool Gnuplot::set_GNUPlotPath(const std::string &path) { std::string tmp = path + "/" + Gnuplot::m_sGNUPlotFileName; @@ -1047,7 +1047,7 @@ bool Gnuplot::set_GNUPlotPath(const std::string &path) // define static member function: set standard terminal, used by showonscreen // defaults: Windows - win, Linux - x11, Mac - aqua // -void Gnuplot::set_terminal_std(const std::string &type) +inline void Gnuplot::set_terminal_std(const std::string &type) { #if defined(unix) || defined(__unix) || defined(__unix__) || defined(__APPLE__) if (type.find("x11") != std::string::npos && std::getenv("DISPLAY") == nullptr) @@ -1128,7 +1128,7 @@ Gnuplot::~Gnuplot() // // Resets a gnuplot session (next plot will erase previous ones) // -Gnuplot &Gnuplot::reset_plot() +inline Gnuplot &Gnuplot::reset_plot() { // remove_tmpfiles(); nplots = 0; @@ -1140,7 +1140,7 @@ Gnuplot &Gnuplot::reset_plot() // // resets a gnuplot session and sets all variables to default // -Gnuplot &Gnuplot::reset_all() +inline Gnuplot &Gnuplot::reset_all() { // remove_tmpfiles(); nplots = 0; @@ -1157,7 +1157,7 @@ Gnuplot &Gnuplot::reset_all() // // Change the plotting style of a gnuplot session // -Gnuplot &Gnuplot::set_style(const std::string &stylestr) +inline Gnuplot &Gnuplot::set_style(const std::string &stylestr) { if (stylestr.find("lines") == std::string::npos && stylestr.find("points") == std::string::npos && @@ -1202,7 +1202,7 @@ Gnuplot &Gnuplot::set_style(const std::string &stylestr) // // smooth: interpolation and approximation of data // -Gnuplot &Gnuplot::set_smooth(const std::string &stylestr) +inline Gnuplot &Gnuplot::set_smooth(const std::string &stylestr) { if (stylestr.find("unique") == std::string::npos && stylestr.find("frequency") == std::string::npos && @@ -1226,7 +1226,7 @@ Gnuplot &Gnuplot::set_smooth(const std::string &stylestr) // // Disable screen output // -Gnuplot &Gnuplot::disablescreen() +inline Gnuplot &Gnuplot::disablescreen() { cmd("set output"); cmd("set terminal unknown"); @@ -1237,7 +1237,7 @@ Gnuplot &Gnuplot::disablescreen() // // sets terminal type to windows / x11 // -Gnuplot &Gnuplot::showonscreen() +inline Gnuplot &Gnuplot::showonscreen() { std::string persist(" persist"); #ifdef __APPLE__ @@ -1254,7 +1254,7 @@ Gnuplot &Gnuplot::showonscreen() // // saves a gnuplot session to a pdf file // -Gnuplot &Gnuplot::savetopdf(const std::string &filename, unsigned int font_size) +inline Gnuplot &Gnuplot::savetopdf(const std::string &filename, unsigned int font_size) { std::ostringstream cmdstr; cmdstr << "set term pdfcairo enhanced color font \"Times-New-Roman," + std::to_string(font_size) + "\"\n"; @@ -1270,7 +1270,7 @@ Gnuplot &Gnuplot::savetopdf(const std::string &filename, unsigned int font_size) // // saves a gnuplot session to a postscript file // -Gnuplot &Gnuplot::savetops(const std::string &filename) +inline Gnuplot &Gnuplot::savetops(const std::string &filename) { std::ostringstream cmdstr; cmdstr << "set term postscript landscape enhanced color dashed \"Times-Roman\" 18\n"; @@ -1286,7 +1286,7 @@ Gnuplot &Gnuplot::savetops(const std::string &filename) // // Switches legend on // -Gnuplot &Gnuplot::set_legend(const std::string &position) +inline Gnuplot &Gnuplot::set_legend(const std::string &position) { std::ostringstream cmdstr; cmdstr << "set key " << position; @@ -1301,7 +1301,7 @@ Gnuplot &Gnuplot::set_legend(const std::string &position) // // turns on log scaling for the x axis // -Gnuplot &Gnuplot::set_xlogscale(const double base) +inline Gnuplot &Gnuplot::set_xlogscale(const double base) { std::ostringstream cmdstr; @@ -1316,7 +1316,7 @@ Gnuplot &Gnuplot::set_xlogscale(const double base) // // turns on log scaling for the y axis // -Gnuplot &Gnuplot::set_ylogscale(const double base) +inline Gnuplot &Gnuplot::set_ylogscale(const double base) { std::ostringstream cmdstr; @@ -1331,7 +1331,7 @@ Gnuplot &Gnuplot::set_ylogscale(const double base) // // turns on log scaling for the z axis // -Gnuplot &Gnuplot::set_zlogscale(const double base) +inline Gnuplot &Gnuplot::set_zlogscale(const double base) { std::ostringstream cmdstr; @@ -1346,7 +1346,7 @@ Gnuplot &Gnuplot::set_zlogscale(const double base) // // scales the size of the points used in plots // -Gnuplot &Gnuplot::set_pointsize(const double pointsize) +inline Gnuplot &Gnuplot::set_pointsize(const double pointsize) { std::ostringstream cmdstr; cmdstr << "set pointsize " << pointsize; @@ -1360,7 +1360,7 @@ Gnuplot &Gnuplot::set_pointsize(const double pointsize) // // set isoline density (grid) for plotting functions as surfaces // -Gnuplot &Gnuplot::set_samples(const int samples) +inline Gnuplot &Gnuplot::set_samples(const int samples) { std::ostringstream cmdstr; cmdstr << "set samples " << samples; @@ -1374,7 +1374,7 @@ Gnuplot &Gnuplot::set_samples(const int samples) // // set isoline density (grid) for plotting functions as surfaces // -Gnuplot &Gnuplot::set_isosamples(const int isolines) +inline Gnuplot &Gnuplot::set_isosamples(const int isolines) { std::ostringstream cmdstr; cmdstr << "set isosamples " << isolines; @@ -1388,7 +1388,7 @@ Gnuplot &Gnuplot::set_isosamples(const int isolines) // // enables contour drawing for surfaces set contour {base | surface | both} // -Gnuplot &Gnuplot::set_contour(const std::string &position) +inline Gnuplot &Gnuplot::set_contour(const std::string &position) { if (position.find("base") == std::string::npos && position.find("surface") == std::string::npos && @@ -1410,7 +1410,7 @@ Gnuplot &Gnuplot::set_contour(const std::string &position) // set labels // // set the xlabel -Gnuplot &Gnuplot::set_xlabel(const std::string &label) +inline Gnuplot &Gnuplot::set_xlabel(const std::string &label) { std::ostringstream cmdstr; @@ -1424,7 +1424,7 @@ Gnuplot &Gnuplot::set_xlabel(const std::string &label) //------------------------------------------------------------------------------ // set the ylabel // -Gnuplot &Gnuplot::set_ylabel(const std::string &label) +inline Gnuplot &Gnuplot::set_ylabel(const std::string &label) { std::ostringstream cmdstr; @@ -1438,7 +1438,7 @@ Gnuplot &Gnuplot::set_ylabel(const std::string &label) //------------------------------------------------------------------------------ // set the zlabel // -Gnuplot &Gnuplot::set_zlabel(const std::string &label) +inline Gnuplot &Gnuplot::set_zlabel(const std::string &label) { std::ostringstream cmdstr; @@ -1454,7 +1454,7 @@ Gnuplot &Gnuplot::set_zlabel(const std::string &label) // set range // // set the xrange -Gnuplot &Gnuplot::set_xrange(const double iFrom, +inline Gnuplot &Gnuplot::set_xrange(const double iFrom, const double iTo) { std::ostringstream cmdstr; @@ -1469,7 +1469,7 @@ Gnuplot &Gnuplot::set_xrange(const double iFrom, //------------------------------------------------------------------------------ // set the yrange // -Gnuplot &Gnuplot::set_yrange(const double iFrom, +inline Gnuplot &Gnuplot::set_yrange(const double iFrom, const double iTo) { std::ostringstream cmdstr; @@ -1484,7 +1484,7 @@ Gnuplot &Gnuplot::set_yrange(const double iFrom, //------------------------------------------------------------------------------ // set the zrange // -Gnuplot &Gnuplot::set_zrange(const double iFrom, +inline Gnuplot &Gnuplot::set_zrange(const double iFrom, const double iTo) { std::ostringstream cmdstr; @@ -1500,7 +1500,7 @@ Gnuplot &Gnuplot::set_zrange(const double iFrom, // // set the palette range // -Gnuplot &Gnuplot::set_cbrange(const double iFrom, +inline Gnuplot &Gnuplot::set_cbrange(const double iFrom, const double iTo) { std::ostringstream cmdstr; @@ -1517,7 +1517,7 @@ Gnuplot &Gnuplot::set_cbrange(const double iFrom, // Plots a linear equation y=ax+b (where you supply the // slope a and intercept b) // -Gnuplot &Gnuplot::plot_slope(const double a, +inline Gnuplot &Gnuplot::plot_slope(const double a, const double b, const std::string &title) { @@ -1560,7 +1560,7 @@ Gnuplot &Gnuplot::plot_slope(const double a, // // Plot an equation supplied as a std::string y=f(x) (only f(x) expected) // -Gnuplot &Gnuplot::plot_equation(const std::string &equation, +inline Gnuplot &Gnuplot::plot_equation(const std::string &equation, const std::string &title) { std::ostringstream cmdstr; @@ -1602,7 +1602,7 @@ Gnuplot &Gnuplot::plot_equation(const std::string &equation, // // plot an equation supplied as a std::string y=(x) // -Gnuplot &Gnuplot::plot_equation3d(const std::string &equation, +inline Gnuplot &Gnuplot::plot_equation3d(const std::string &equation, const std::string &title) { std::ostringstream cmdstr; @@ -1644,7 +1644,7 @@ Gnuplot &Gnuplot::plot_equation3d(const std::string &equation, // // Plots a 2d graph from a list of doubles (x) saved in a file // -Gnuplot &Gnuplot::plotfile_x(const std::string &filename, +inline Gnuplot &Gnuplot::plotfile_x(const std::string &filename, const unsigned int column, const std::string &title) { @@ -1699,7 +1699,7 @@ Gnuplot &Gnuplot::plotfile_x(const std::string &filename, // // Plots a 2d graph from a list of doubles (x y) saved in a file // -Gnuplot &Gnuplot::plotfile_xy(const std::string &filename, +inline Gnuplot &Gnuplot::plotfile_xy(const std::string &filename, const unsigned int column_x, const unsigned int column_y, const std::string &title, @@ -1756,7 +1756,7 @@ Gnuplot &Gnuplot::plotfile_xy(const std::string &filename, // // Plots a 2d graph with errorbars from a list of doubles (x y dy) in a file // -Gnuplot &Gnuplot::plotfile_xy_err(const std::string &filename, +inline Gnuplot &Gnuplot::plotfile_xy_err(const std::string &filename, const unsigned int column_x, const unsigned int column_y, const unsigned int column_dy, @@ -1806,7 +1806,7 @@ Gnuplot &Gnuplot::plotfile_xy_err(const std::string &filename, // // Plots a 3d graph from a list of doubles (x y z) saved in a file // -Gnuplot &Gnuplot::plotfile_xyz(const std::string &filename, +inline Gnuplot &Gnuplot::plotfile_xyz(const std::string &filename, const unsigned int column_x, const unsigned int column_y, const unsigned int column_z, @@ -1855,7 +1855,7 @@ Gnuplot &Gnuplot::plotfile_xyz(const std::string &filename, // /// * note that this function is not valid for versions of GNUPlot below 4.2 // -Gnuplot &Gnuplot::plot_image(const unsigned char *ucPicBuf, +inline Gnuplot &Gnuplot::plot_image(const unsigned char *ucPicBuf, const unsigned int iWidth, const unsigned int iHeight, const std::string &title) @@ -1914,7 +1914,7 @@ Gnuplot &Gnuplot::plot_image(const unsigned char *ucPicBuf, } -Gnuplot &Gnuplot::plot_circle(double east, double north, double radius, const std::string &label) +inline Gnuplot &Gnuplot::plot_circle(double east, double north, double radius, const std::string &label) { std::ostringstream cmdstr; // @@ -1952,7 +1952,7 @@ Gnuplot &Gnuplot::plot_circle(double east, double north, double radius, const st // // Sends a command to an active gnuplot session // -Gnuplot &Gnuplot::cmd(const std::string &cmdstr) +inline Gnuplot &Gnuplot::cmd(const std::string &cmdstr) { if (!(valid)) { @@ -1996,7 +1996,7 @@ Gnuplot &Gnuplot::cmd(const std::string &cmdstr) // // Opens up a gnuplot session, ready to receive commands // -void Gnuplot::init() +inline void Gnuplot::init() { // char * getenv ( const char * name ); get value of environment variable // Retrieves a C string containing the value of the environment variable @@ -2058,7 +2058,7 @@ void Gnuplot::init() // // Find out if a command lives in m_sGNUPlotPath or in PATH // -bool Gnuplot::get_program_path() +inline bool Gnuplot::get_program_path() { // // first look in m_sGNUPlotPath for Gnuplot @@ -2129,7 +2129,7 @@ bool Gnuplot::get_program_path() // // check if file exists // -bool Gnuplot::file_exists(const std::string &filename, int mode) +inline bool Gnuplot::file_exists(const std::string &filename, int mode) { if (mode < 0 || mode > 7) { @@ -2161,7 +2161,7 @@ bool Gnuplot::file_exists(const std::string &filename, int mode) } -bool Gnuplot::file_available(const std::string &filename) +inline bool Gnuplot::file_available(const std::string &filename) { std::ostringstream except; if (Gnuplot::file_exists(filename, 0)) // check existence @@ -2187,7 +2187,7 @@ bool Gnuplot::file_available(const std::string &filename) // // Opens a temporary file // -std::string Gnuplot::create_tmpfile(std::ofstream &tmp) +inline std::string Gnuplot::create_tmpfile(std::ofstream &tmp) { #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) char name[] = "gnuplotiXXXXXX"; //tmp file in working directory @@ -2256,7 +2256,7 @@ std::string Gnuplot::create_tmpfile(std::ofstream &tmp) } -void Gnuplot::remove_tmpfiles() +inline void Gnuplot::remove_tmpfiles() { if (!(tmpfile_list).empty()) { From d5e5e5725d8faea3b0e9dd657836e7bba1dd4c0e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 00:01:54 +0200 Subject: [PATCH 18/64] Apply fixes by clang-tidy --- src/algorithms/PVT/libs/rtcm.cc | 22 ++++---- .../adapters/beidou_b3i_pcps_acquisition.cc | 5 +- .../gps_l1_ca_pcps_opencl_acquisition.cc | 5 +- .../libs/galileo_e1_signal_processing.cc | 2 +- .../adapters/custom_udp_signal_source.cc | 4 +- .../gr_complex_ip_packet_source.cc | 15 ++++-- .../adapters/beidou_b3i_dll_pll_tracking.cc | 10 +++- src/core/receiver/control_thread.cc | 16 +++--- src/core/receiver/gnss_flowgraph.cc | 50 +++++++++++-------- .../gps_l1_ca_pcps_acquisition_test.cc | 4 +- .../adapter/adapter_test.cc | 2 +- .../filter/fir_filter_test.cc | 2 +- .../filter/notch_filter_lite_test.cc | 2 +- .../filter/notch_filter_test.cc | 2 +- .../filter/pulse_blanking_filter_test.cc | 2 +- .../tracking/cubature_filter_test.cc | 4 +- .../galileo_e1_dll_pll_veml_tracking_test.cc | 2 +- .../tracking/unscented_filter_test.cc | 4 +- src/utils/front-end-cal/main.cc | 2 +- 19 files changed, 91 insertions(+), 64 deletions(-) diff --git a/src/algorithms/PVT/libs/rtcm.cc b/src/algorithms/PVT/libs/rtcm.cc index e514d733a..0cad7ec65 100644 --- a/src/algorithms/PVT/libs/rtcm.cc +++ b/src/algorithms/PVT/libs/rtcm.cc @@ -2419,7 +2419,7 @@ std::string Rtcm::get_MSM_1_content_sat_data(const std::mapsecond.PRN); - observables_vector.push_back(*gnss_synchro_iter); + observables_vector.emplace_back(*gnss_synchro_iter); } } @@ -2448,7 +2448,7 @@ std::string Rtcm::get_MSM_1_content_signal_data(const std::map > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -2556,7 +2556,7 @@ std::string Rtcm::get_MSM_2_content_signal_data(const Gps_Ephemeris& ephNAV, map_iter != observables.cend(); map_iter++) { - observables_vector.push_back(*map_iter); + observables_vector.emplace_back(*map_iter); } std::vector > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -2670,7 +2670,7 @@ std::string Rtcm::get_MSM_3_content_signal_data(const Gps_Ephemeris& ephNAV, map_iter != observables.cend(); map_iter++) { - observables_vector.push_back(*map_iter); + observables_vector.emplace_back(*map_iter); } std::vector > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -2786,7 +2786,7 @@ std::string Rtcm::get_MSM_4_content_sat_data(const std::mapsecond.PRN); - observables_vector.push_back(*gnss_synchro_iter); + observables_vector.emplace_back(*gnss_synchro_iter); } } @@ -2827,7 +2827,7 @@ std::string Rtcm::get_MSM_4_content_signal_data(const Gps_Ephemeris& ephNAV, map_iter != observables.cend(); map_iter++) { - observables_vector.push_back(*map_iter); + observables_vector.emplace_back(*map_iter); } std::vector > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -2947,7 +2947,7 @@ std::string Rtcm::get_MSM_5_content_sat_data(const std::mapsecond.PRN); - observables_vector.push_back(*gnss_synchro_iter); + observables_vector.emplace_back(*gnss_synchro_iter); } } @@ -2993,7 +2993,7 @@ std::string Rtcm::get_MSM_5_content_signal_data(const Gps_Ephemeris& ephNAV, map_iter != observables.cend(); map_iter++) { - observables_vector.push_back(*map_iter); + observables_vector.emplace_back(*map_iter); } std::vector > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -3114,7 +3114,7 @@ std::string Rtcm::get_MSM_6_content_signal_data(const Gps_Ephemeris& ephNAV, map_iter != observables.cend(); map_iter++) { - observables_vector.push_back(*map_iter); + observables_vector.emplace_back(*map_iter); } std::vector > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -3234,7 +3234,7 @@ std::string Rtcm::get_MSM_7_content_signal_data(const Gps_Ephemeris& ephNAV, map_iter != observables.cend(); map_iter++) { - observables_vector.push_back(*map_iter); + observables_vector.emplace_back(*map_iter); } std::vector > ordered_by_signal = Rtcm::sort_by_signal(observables_vector); @@ -3685,7 +3685,7 @@ uint32_t Rtcm::msm_extended_lock_time_indicator(uint32_t lock_time_period_s) if( 16777216 <= lock_time_period_s && lock_time_period_s < 33554432 ) return (640 + (lock_time_period_s - 16777216) / 524288 ); if( 33554432 <= lock_time_period_s && lock_time_period_s < 67108864 ) return (672 + (lock_time_period_s - 33554432) / 1048576); if( 67108864 <= lock_time_period_s ) return (704 ); - return 1023; // will never happen + return 1023; // will never happen } // clang-format on diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc index b6bf5f7ed..6c86d08f1 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc @@ -66,7 +66,10 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( blocking_ = configuration_->property(role + ".blocking", true); acq_parameters.blocking = blocking_; doppler_max_ = configuration_->property(role + ".doppler_max", 5000); - if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max; + if (FLAGS_doppler_max != 0) + { + doppler_max_ = FLAGS_doppler_max; + } acq_parameters.doppler_max = doppler_max_; sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); acq_parameters.sampled_ms = sampled_ms_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc index f96a29632..f5d193d13 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc @@ -60,7 +60,10 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition( fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); dump_ = configuration_->property(role + ".dump", false); doppler_max_ = configuration->property(role + ".doppler_max", 5000); - if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max; + if (FLAGS_doppler_max != 0) + { + doppler_max_ = FLAGS_doppler_max; + } sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); bit_transition_flag_ = configuration_->property("Acquisition.bit_transition_flag", false); diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index 085fe8e78..1bf97f3b2 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -217,7 +217,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< { for (unsigned k = 0; k < _samplesPerCode; k++) { - _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span_aux[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f); + _signal_E1C_secondary_span[i * _samplesPerCode + k] = _signal_E1_span_aux[k] * (GALILEO_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0F : -1.0F); } } diff --git a/src/algorithms/signal_source/adapters/custom_udp_signal_source.cc b/src/algorithms/signal_source/adapters/custom_udp_signal_source.cc index d030863ff..024966d0e 100644 --- a/src/algorithms/signal_source/adapters/custom_udp_signal_source.cc +++ b/src/algorithms/signal_source/adapters/custom_udp_signal_source.cc @@ -81,7 +81,7 @@ CustomUDPSignalSource::CustomUDPSignalSource(ConfigurationInterface* configurati { for (int n = 0; n < channels_in_udp_; n++) { - null_sinks_.push_back(gr::blocks::null_sink::make(sizeof(gr_complex))); + null_sinks_.emplace_back(gr::blocks::null_sink::make(sizeof(gr_complex))); } } else @@ -95,7 +95,7 @@ CustomUDPSignalSource::CustomUDPSignalSource(ConfigurationInterface* configurati for (int n = 0; n < channels_in_udp_; n++) { DLOG(INFO) << "Dumping output into file " << (dump_filename_ + "c_h" + std::to_string(n) + ".bin"); - file_sink_.push_back(gr::blocks::file_sink::make(item_size_, (dump_filename_ + "_ch" + std::to_string(n) + ".bin").c_str())); + file_sink_.emplace_back(gr::blocks::file_sink::make(item_size_, (dump_filename_ + "_ch" + std::to_string(n) + ".bin").c_str())); } } if (in_stream_ > 0) diff --git a/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.cc b/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.cc index 8085e9e81..ac52e0236 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.cc @@ -290,7 +290,10 @@ void Gr_Complex_Ip_Packet_Source::pcap_callback(__attribute__((unused)) u_char * // write all in a single memcpy memcpy(&fifo_buff[fifo_write_ptr], &udp_payload[0], payload_length_bytes); // size in bytes fifo_write_ptr += payload_length_bytes; - if (fifo_write_ptr == FIFO_SIZE) fifo_write_ptr = 0; + if (fifo_write_ptr == FIFO_SIZE) + { + fifo_write_ptr = 0; + } fifo_items += payload_length_bytes; } else @@ -378,7 +381,10 @@ void Gr_Complex_Ip_Packet_Source::demux_samples(gr_vector_void_star output_items std::cout << "Unknown wire sample type\n"; exit(0); } - if (fifo_read_ptr == FIFO_SIZE) fifo_read_ptr = 0; + if (fifo_read_ptr == FIFO_SIZE) + { + fifo_read_ptr = 0; + } } } @@ -389,7 +395,10 @@ int Gr_Complex_Ip_Packet_Source::work(int noutput_items, { // send samples to next GNU Radio block boost::mutex::scoped_lock lock(d_mutex); // hold mutex for duration of this function - if (fifo_items == 0) return 0; + if (fifo_items == 0) + { + return 0; + } if (output_items.size() > static_cast(d_n_baseband_channels)) { diff --git a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc index 306a0efb5..fb998ab81 100644 --- a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc @@ -59,14 +59,20 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + if (FLAGS_pll_bw_hz != 0.0) + { + pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + } trk_param.pll_bw_hz = pll_bw_hz; float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); - if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + if (FLAGS_dll_bw_hz != 0.0) + { + dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + } trk_param.dll_bw_hz = dll_bw_hz; int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index e92646586..384d6f041 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -933,8 +933,8 @@ std::vector> ControlThread::get_visible_sats(time if (El > 0) { std::cout << "Using GPS Ephemeris: Sat " << it.second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; - available_satellites.push_back(std::pair(floor(El), - (Gnss_Satellite(std::string("GPS"), it.second.i_satellite_PRN)))); + available_satellites.emplace_back(floor(El), + (Gnss_Satellite(std::string("GPS"), it.second.i_satellite_PRN))); visible_gps.push_back(it.second.i_satellite_PRN); } } @@ -956,8 +956,8 @@ std::vector> ControlThread::get_visible_sats(time if (El > 0) { std::cout << "Using Galileo Ephemeris: Sat " << it.second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; - available_satellites.push_back(std::pair(floor(El), - (Gnss_Satellite(std::string("Galileo"), it.second.i_satellite_PRN)))); + available_satellites.emplace_back(floor(El), + (Gnss_Satellite(std::string("Galileo"), it.second.i_satellite_PRN))); visible_gal.push_back(it.second.i_satellite_PRN); } } @@ -984,8 +984,8 @@ std::vector> ControlThread::get_visible_sats(time if (it2 == visible_gps.end()) { std::cout << "Using GPS Almanac: Sat " << it.second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; - available_satellites.push_back(std::pair(floor(El), - (Gnss_Satellite(std::string("GPS"), it.second.i_satellite_PRN)))); + available_satellites.emplace_back(floor(El), + (Gnss_Satellite(std::string("GPS"), it.second.i_satellite_PRN))); } } } @@ -1012,8 +1012,8 @@ std::vector> ControlThread::get_visible_sats(time if (it2 == visible_gal.end()) { std::cout << "Using Galileo Almanac: Sat " << it.second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; - available_satellites.push_back(std::pair(floor(El), - (Gnss_Satellite(std::string("Galileo"), it.second.i_satellite_PRN)))); + available_satellites.emplace_back(floor(El), + (Gnss_Satellite(std::string("Galileo"), it.second.i_satellite_PRN))); } } } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 1de87bc17..51e805957 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1854,9 +1854,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_gps_prn.cend(); available_gnss_prn_iter++) { - available_GPS_1C_signals_.push_back(Gnss_Signal( + available_GPS_1C_signals_.emplace_back( Gnss_Satellite(std::string("GPS"), *available_gnss_prn_iter), - std::string("1C"))); + std::string("1C")); } } @@ -1867,9 +1867,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_gps_prn.cend(); available_gnss_prn_iter++) { - available_GPS_2S_signals_.push_back(Gnss_Signal( + available_GPS_2S_signals_.emplace_back( Gnss_Satellite(std::string("GPS"), *available_gnss_prn_iter), - std::string("2S"))); + std::string("2S")); } } @@ -1880,9 +1880,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_gps_prn.cend(); available_gnss_prn_iter++) { - available_GPS_L5_signals_.push_back(Gnss_Signal( + available_GPS_L5_signals_.emplace_back( Gnss_Satellite(std::string("GPS"), *available_gnss_prn_iter), - std::string("L5"))); + std::string("L5")); } } @@ -1893,9 +1893,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_sbas_prn.cend(); available_gnss_prn_iter++) { - available_SBAS_1C_signals_.push_back(Gnss_Signal( + available_SBAS_1C_signals_.emplace_back( Gnss_Satellite(std::string("SBAS"), *available_gnss_prn_iter), - std::string("1C"))); + std::string("1C")); } } @@ -1906,9 +1906,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_galileo_prn.cend(); available_gnss_prn_iter++) { - available_GAL_1B_signals_.push_back(Gnss_Signal( + available_GAL_1B_signals_.emplace_back( Gnss_Satellite(std::string("Galileo"), *available_gnss_prn_iter), - std::string("1B"))); + std::string("1B")); } } @@ -1919,9 +1919,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_galileo_prn.cend(); available_gnss_prn_iter++) { - available_GAL_5X_signals_.push_back(Gnss_Signal( + available_GAL_5X_signals_.emplace_back( Gnss_Satellite(std::string("Galileo"), *available_gnss_prn_iter), - std::string("5X"))); + std::string("5X")); } } @@ -1932,9 +1932,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_glonass_prn.cend(); available_gnss_prn_iter++) { - available_GLO_1G_signals_.push_back(Gnss_Signal( + available_GLO_1G_signals_.emplace_back( Gnss_Satellite(std::string("Glonass"), *available_gnss_prn_iter), - std::string("1G"))); + std::string("1G")); } } @@ -1945,9 +1945,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_glonass_prn.cend(); available_gnss_prn_iter++) { - available_GLO_2G_signals_.push_back(Gnss_Signal( + available_GLO_2G_signals_.emplace_back( Gnss_Satellite(std::string("Glonass"), *available_gnss_prn_iter), - std::string("2G"))); + std::string("2G")); } } @@ -1960,9 +1960,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_beidou_prn.cend(); available_gnss_prn_iter++) { - available_BDS_B1_signals_.push_back(Gnss_Signal( + available_BDS_B1_signals_.emplace_back( Gnss_Satellite(std::string("Beidou"), *available_gnss_prn_iter), - std::string("B1"))); + std::string("B1")); } } @@ -1975,9 +1975,9 @@ void GNSSFlowgraph::set_signals_list() available_gnss_prn_iter != available_beidou_prn.cend(); available_gnss_prn_iter++) { - available_BDS_B3_signals_.push_back(Gnss_Signal( + available_BDS_B3_signals_.emplace_back( Gnss_Satellite(std::string("Beidou"), *available_gnss_prn_iter), - std::string("B3"))); + std::string("B3")); } } } @@ -2243,7 +2243,10 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal { for (unsigned int ch = 0; ch < channels_count_; ch++) { - if ((channels_[ch]->get_signal().get_satellite() == result.get_satellite()) and (channels_[ch]->get_signal().get_signal_str() != "2G")) untracked_satellite = false; + if ((channels_[ch]->get_signal().get_satellite() == result.get_satellite()) and (channels_[ch]->get_signal().get_signal_str() != "2G")) + { + untracked_satellite = false; + } } if (untracked_satellite) { @@ -2268,7 +2271,10 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal { for (unsigned int ch = 0; ch < channels_count_; ch++) { - if ((channels_[ch]->get_signal().get_satellite() == result.get_satellite()) and (channels_[ch]->get_signal().get_signal_str() != "2G")) untracked_satellite = false; + if ((channels_[ch]->get_signal().get_satellite() == result.get_satellite()) and (channels_[ch]->get_signal().get_signal_str() != "2G")) + { + untracked_satellite = false; + } } if (untracked_satellite) { diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc index 01f424eb8..0ec417ee0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc @@ -88,7 +88,7 @@ private: public: int rx_message; - ~GpsL1CaPcpsAcquisitionTest_msg_rx(); //!< Default destructor + ~GpsL1CaPcpsAcquisitionTest_msg_rx() override; //!< Default destructor }; @@ -139,7 +139,7 @@ protected: doppler_step = 100; } - ~GpsL1CaPcpsAcquisitionTest() = default; + ~GpsL1CaPcpsAcquisitionTest() override = default; void init(); void plot_grid(); diff --git a/src/tests/unit-tests/signal-processing-blocks/adapter/adapter_test.cc b/src/tests/unit-tests/signal-processing-blocks/adapter/adapter_test.cc index 89f2cd338..7c0677825 100644 --- a/src/tests/unit-tests/signal-processing-blocks/adapter/adapter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/adapter/adapter_test.cc @@ -51,7 +51,7 @@ class DataTypeAdapter : public ::testing::Test { public: DataTypeAdapter(); - ~DataTypeAdapter(); + ~DataTypeAdapter() override; int run_byte_to_short_block(); int run_ibyte_to_cbyte_block(); int run_ibyte_to_complex_block(); diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc index 42fb1496a..03d055fac 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc @@ -63,7 +63,7 @@ protected: item_size = sizeof(gr_complex); config = std::make_shared(); } - ~FirFilterTest() = default; + ~FirFilterTest() override = default; void init(); void configure_cbyte_cbyte(); diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc index a77e9d853..69c1dddf9 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc @@ -62,7 +62,7 @@ protected: config = std::make_shared(); nsamples = FLAGS_notch_filter_lite_test_nsamples; } - ~NotchFilterLiteTest() = default; + ~NotchFilterLiteTest() override = default; void init(); void configure_gr_complex_gr_complex(); diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc index 943ac2fe2..8fb783b5f 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc @@ -62,7 +62,7 @@ protected: config = std::make_shared(); nsamples = FLAGS_notch_filter_test_nsamples; } - ~NotchFilterTest() = default; + ~NotchFilterTest() override = default; void init(); void configure_gr_complex_gr_complex(); diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc index 83a63d551..d31b971fb 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc @@ -62,7 +62,7 @@ protected: config = std::make_shared(); nsamples = FLAGS_pb_filter_test_nsamples; } - ~PulseBlankingFilterTest() = default; + ~PulseBlankingFilterTest() override = default; void init(); void configure_gr_complex_gr_complex(); diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc index 0833b12be..a0d42aeb2 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc @@ -40,7 +40,7 @@ class TransitionModel : public ModelFunction { public: explicit TransitionModel(const arma::mat& kf_F) { coeff_mat = kf_F; }; - virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; + arma::vec operator()(const arma::vec& input) override { return coeff_mat * input; }; private: arma::mat coeff_mat; @@ -50,7 +50,7 @@ class MeasurementModel : public ModelFunction { public: explicit MeasurementModel(const arma::mat& kf_H) { coeff_mat = kf_H; }; - virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; + arma::vec operator()(const arma::vec& input) override { return coeff_mat * input; }; private: arma::mat coeff_mat; diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc index 897257b93..469ec2c27 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc @@ -64,7 +64,7 @@ protected: gnss_synchro = Gnss_Synchro(); } - ~GalileoE1DllPllVemlTrackingInternalTest() = default; + ~GalileoE1DllPllVemlTrackingInternalTest() override = default; void init(); diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc index bc9e5315d..61ae638d0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc @@ -40,7 +40,7 @@ class TransitionModelUKF : public ModelFunction { public: explicit TransitionModelUKF(const arma::mat& kf_F) { coeff_mat = kf_F; }; - virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; + arma::vec operator()(const arma::vec& input) override { return coeff_mat * input; }; private: arma::mat coeff_mat; @@ -50,7 +50,7 @@ class MeasurementModelUKF : public ModelFunction { public: explicit MeasurementModelUKF(const arma::mat& kf_H) { coeff_mat = kf_H; }; - virtual arma::vec operator()(const arma::vec& input) { return coeff_mat * input; }; + arma::vec operator()(const arma::vec& input) override { return coeff_mat * input; }; private: arma::mat coeff_mat; diff --git a/src/utils/front-end-cal/main.cc b/src/utils/front-end-cal/main.cc index 83b664362..54723dfc5 100644 --- a/src/utils/front-end-cal/main.cc +++ b/src/utils/front-end-cal/main.cc @@ -122,7 +122,7 @@ private: public: int rx_message; - ~FrontEndCal_msg_rx(); //!< Default destructor + ~FrontEndCal_msg_rx() override; //!< Default destructor }; From 17b1ef8b98b2597f9e62d969e763e17990ddc56a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 04:47:15 +0200 Subject: [PATCH 19/64] Do not use deletes. Improve memory management --- .../adapters/beidou_b1i_pcps_acquisition.cc | 17 ++++----- .../adapters/beidou_b1i_pcps_acquisition.h | 11 +++--- .../adapters/beidou_b3i_pcps_acquisition.cc | 16 ++++----- .../adapters/beidou_b3i_pcps_acquisition.h | 11 +++--- ...lileo_e1_pcps_8ms_ambiguous_acquisition.cc | 18 ++++------ ...alileo_e1_pcps_8ms_ambiguous_acquisition.h | 4 ++- .../galileo_e1_pcps_ambiguous_acquisition.cc | 26 +++++++------- .../galileo_e1_pcps_ambiguous_acquisition.h | 11 +++--- ...ileo_e1_pcps_ambiguous_acquisition_fpga.cc | 23 ++++++------ ...lileo_e1_pcps_ambiguous_acquisition_fpga.h | 9 ++--- ...eo_e1_pcps_cccwsr_ambiguous_acquisition.cc | 18 +++++----- ...leo_e1_pcps_cccwsr_ambiguous_acquisition.h | 12 ++++--- ...e1_pcps_quicksync_ambiguous_acquisition.cc | 21 +++++------ ..._e1_pcps_quicksync_ambiguous_acquisition.h | 4 ++- ...ileo_e1_pcps_tong_ambiguous_acquisition.cc | 20 +++++------ ...lileo_e1_pcps_tong_ambiguous_acquisition.h | 4 ++- ...ileo_e5a_noncoherent_iq_acquisition_caf.cc | 36 ++++++++----------- ...lileo_e5a_noncoherent_iq_acquisition_caf.h | 11 +++--- .../adapters/galileo_e5a_pcps_acquisition.cc | 20 +++++------ .../adapters/galileo_e5a_pcps_acquisition.h | 25 ++++--------- .../galileo_e5a_pcps_acquisition_fpga.cc | 18 ++++------ .../galileo_e5a_pcps_acquisition_fpga.h | 13 +++---- .../glonass_l1_ca_pcps_acquisition.cc | 17 ++++----- .../adapters/glonass_l1_ca_pcps_acquisition.h | 11 +++--- .../glonass_l2_ca_pcps_acquisition.cc | 16 ++++----- .../adapters/glonass_l2_ca_pcps_acquisition.h | 10 +++--- .../adapters/gps_l1_ca_pcps_acquisition.cc | 17 ++++----- .../adapters/gps_l1_ca_pcps_acquisition.h | 12 +++---- ...gps_l1_ca_pcps_acquisition_fine_doppler.cc | 11 +++--- .../gps_l1_ca_pcps_acquisition_fine_doppler.h | 9 +++-- .../gps_l1_ca_pcps_acquisition_fpga.cc | 18 ++++------ .../gps_l1_ca_pcps_acquisition_fpga.h | 6 ++-- .../gps_l1_ca_pcps_assisted_acquisition.cc | 11 +++--- .../gps_l1_ca_pcps_assisted_acquisition.h | 10 +++--- .../gps_l1_ca_pcps_opencl_acquisition.cc | 18 ++++------ .../gps_l1_ca_pcps_opencl_acquisition.h | 10 +++--- .../gps_l1_ca_pcps_quicksync_acquisition.cc | 18 ++++------ .../gps_l1_ca_pcps_quicksync_acquisition.h | 10 +++--- .../gps_l1_ca_pcps_tong_acquisition.cc | 19 +++++----- .../gps_l1_ca_pcps_tong_acquisition.h | 10 +++--- .../adapters/gps_l2_m_pcps_acquisition.cc | 20 +++++------ .../adapters/gps_l2_m_pcps_acquisition.h | 12 +++---- .../gps_l2_m_pcps_acquisition_fpga.cc | 22 +++++------- .../adapters/gps_l2_m_pcps_acquisition_fpga.h | 11 +++--- .../adapters/gps_l5i_pcps_acquisition.cc | 22 ++++++------ .../adapters/gps_l5i_pcps_acquisition.h | 17 ++++----- .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 19 ++++------ .../adapters/gps_l5i_pcps_acquisition_fpga.h | 10 +++--- .../libs/galileo_e1_signal_processing.cc | 16 ++++----- .../libs/galileo_e5_signal_processing.cc | 19 +++------- src/algorithms/libs/gps_l2c_signal.cc | 12 +++---- src/algorithms/libs/gps_l5_signal.cc | 22 ++++-------- 52 files changed, 340 insertions(+), 443 deletions(-) diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc index c911af3d4..1c266882b 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc @@ -40,6 +40,7 @@ #include #include #include +#include BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( @@ -91,7 +92,7 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( vector_length_ *= 2; } - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -136,10 +137,7 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( } -BeidouB1iPcpsAcquisition::~BeidouB1iPcpsAcquisition() -{ - delete[] code_; -} +BeidouB1iPcpsAcquisition::~BeidouB1iPcpsAcquisition() = default; void BeidouB1iPcpsAcquisition::stop_acquisition() @@ -205,18 +203,17 @@ void BeidouB1iPcpsAcquisition::init() void BeidouB1iPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; beidou_b1i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h index f274d3ee6..503b6409e 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h @@ -42,7 +42,9 @@ #include #include #include +#include #include +#include class ConfigurationInterface; @@ -100,15 +102,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } - /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -159,7 +160,6 @@ public: */ void set_resampler_latency(uint32_t latency_samples) override; - private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; @@ -183,12 +183,11 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; uint32_t in_streams_; uint32_t out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc index 6c86d08f1..3a51a2a77 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc @@ -91,7 +91,7 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( vector_length_ *= 2; } - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -136,10 +136,7 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( } -BeidouB3iPcpsAcquisition::~BeidouB3iPcpsAcquisition() -{ - delete[] code_; -} +BeidouB3iPcpsAcquisition::~BeidouB3iPcpsAcquisition() = default; void BeidouB3iPcpsAcquisition::stop_acquisition() @@ -205,18 +202,17 @@ void BeidouB3iPcpsAcquisition::init() void BeidouB3iPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; beidou_b3i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h index d824c132a..db2c6fbaa 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h @@ -41,7 +41,9 @@ #include #include #include +#include #include +#include class ConfigurationInterface; @@ -99,15 +101,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } - /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -158,7 +159,6 @@ public: */ void set_resampler_latency(uint32_t latency_samples) override; - private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; @@ -182,12 +182,11 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc index 330016bd2..431a8142c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc @@ -87,7 +87,7 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition( int samples_per_ms = code_length_ / 4; - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -123,10 +123,7 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition( } -GalileoE1Pcps8msAmbiguousAcquisition::~GalileoE1Pcps8msAmbiguousAcquisition() -{ - delete[] code_; -} +GalileoE1Pcps8msAmbiguousAcquisition::~GalileoE1Pcps8msAmbiguousAcquisition() = default; void GalileoE1Pcps8msAmbiguousAcquisition::stop_acquisition() @@ -217,22 +214,20 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_local_code() bool cboc = configuration_->property( "Acquisition" + std::to_string(channel_) + ".cboc", false); - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; std::array Signal_; std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_cc_->set_local_code(code_); - - delete[] code; + acquisition_cc_->set_local_code(code_.data()); } } @@ -245,6 +240,7 @@ void GalileoE1Pcps8msAmbiguousAcquisition::reset() } } + float GalileoE1Pcps8msAmbiguousAcquisition::calculate_threshold(float pfa) { unsigned int frequency_bins = 0; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.h index 27e9049de..a3be14fce 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.h @@ -36,7 +36,9 @@ #include "galileo_pcps_8ms_acquisition_cc.h" #include "gnss_synchro.h" #include +#include #include +#include class ConfigurationInterface; @@ -165,7 +167,7 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index f0cfce6a3..60082fc4c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -127,7 +127,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( vector_length_ *= 2; } - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -167,16 +167,14 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( } -GalileoE1PcpsAmbiguousAcquisition::~GalileoE1PcpsAmbiguousAcquisition() -{ - delete[] code_; -} +GalileoE1PcpsAmbiguousAcquisition::~GalileoE1PcpsAmbiguousAcquisition() = default; void GalileoE1PcpsAmbiguousAcquisition::stop_acquisition() { } + void GalileoE1PcpsAmbiguousAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); @@ -243,7 +241,8 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() bool cboc = configuration_->property( "Acquisition" + std::to_string(channel_) + ".cboc", false); - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; + gsl::span> code_span(code.get(), code_length_); if (acquire_pilot_ == true) { @@ -251,12 +250,12 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() std::array pilot_signal = {{'1', 'C', '\0'}}; if (acq_parameters_.use_automatic_resampler) { - galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), pilot_signal, + galileo_e1_code_gen_complex_sampled(code_span, pilot_signal, cboc, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0, false); } else { - galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), pilot_signal, + galileo_e1_code_gen_complex_sampled(code_span, pilot_signal, cboc, gnss_synchro_->PRN, fs_in_, 0, false); } } @@ -266,24 +265,23 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); if (acq_parameters_.use_automatic_resampler) { - galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, + galileo_e1_code_gen_complex_sampled(code_span, Signal_, cboc, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0, false); } else { - galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, + galileo_e1_code_gen_complex_sampled(code_span, Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); } } - gsl::span code_span(code_, vector_length_); + gsl::span code__span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code__span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h index 12aebab73..92c25724b 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h @@ -39,7 +39,9 @@ #include "pcps_acquisition.h" #include #include +#include #include +#include class ConfigurationInterface; @@ -98,13 +100,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -153,10 +156,8 @@ public: /*! * \brief Sets the resampler latency to account it in the acquisition code delay estimation */ - void set_resampler_latency(uint32_t latency_samples) override; - private: ConfigurationInterface* configuration_; Acq_Conf acq_parameters_; @@ -181,7 +182,7 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index 24e275ff6..0742b0ff5 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -109,10 +109,11 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( // compute all the GALILEO E1 PRN Codes (this is done only once in the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT - auto* code = new std::complex[nsamples_total]; // buffer for the local code + //auto code = std::make_shared>(nsamples_total); // buffer for the local code + std::vector> code(nsamples_total); auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new uint32_t[(nsamples_total * GALILEO_E1_NUMBER_OF_CODES)]; // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + d_all_fft_codes_ = std::vector(nsamples_total * GALILEO_E1_NUMBER_OF_CODES); // memory containing all the possible fft codes for PRN 0 to 32 + float max; // temporary maxima search int32_t tmp, tmp2, local_code, fft_data; for (uint32_t PRN = 1; PRN <= GALILEO_E1_NUMBER_OF_CODES; PRN++) @@ -123,13 +124,13 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( { //set local signal generator to Galileo E1 pilot component (1C) std::array pilot_signal = {{'1', 'C', '\0'}}; - galileo_e1_code_gen_complex_sampled(gsl::span>(code, nsamples_total), pilot_signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code.data(), nsamples_total), pilot_signal, cboc, PRN, fs_in, 0, false); } else { std::array data_signal = {{'1', 'B', '\0'}}; - galileo_e1_code_gen_complex_sampled(gsl::span>(code, nsamples_total), data_signal, + galileo_e1_code_gen_complex_sampled(gsl::span>(code.data(), nsamples_total), data_signal, cboc, PRN, fs_in, 0, false); } @@ -144,7 +145,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( code[s] = std::complex(0.0, 0.0); } - std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer + std::copy_n(code.data(), nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values @@ -173,7 +174,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( } } - acq_parameters.all_fft_codes = d_all_fft_codes_; + acq_parameters.all_fft_codes = d_all_fft_codes_.data(); acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); @@ -189,16 +190,12 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( gnss_synchro_ = nullptr; // temporary buffers that we can delete - delete[] code; delete fft_if; - delete[] fft_codes_padded; + volk_gnsssdr_free(fft_codes_padded); } -GalileoE1PcpsAmbiguousAcquisitionFpga::~GalileoE1PcpsAmbiguousAcquisitionFpga() -{ - delete[] d_all_fft_codes_; -} +GalileoE1PcpsAmbiguousAcquisitionFpga::~GalileoE1PcpsAmbiguousAcquisitionFpga() = default; void GalileoE1PcpsAmbiguousAcquisitionFpga::stop_acquisition() diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index 83c3f66f5..a92b65e31 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -37,7 +37,9 @@ #include // for basic_block_sptr, top_block_sptr #include // for lv_16sc_t #include // for size_t +#include #include +#include class Gnss_Synchro; class ConfigurationInterface; @@ -97,8 +99,8 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; @@ -165,8 +167,7 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - - uint32_t* d_all_fft_codes_; // memory that contains all the code ffts + std::vector d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GALILEO_E1_PCPS_AMBIGUOUS_ACQUISITION_FPGA_H_ */ diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc index d29e896a5..f16c6e5da 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc @@ -86,8 +86,8 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition int samples_per_ms = code_length_ / 4; - code_data_ = new gr_complex[vector_length_]; - code_pilot_ = new gr_complex[vector_length_]; + code_data_ = std::vector>(vector_length_); + code_pilot_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -123,11 +123,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition } -GalileoE1PcpsCccwsrAmbiguousAcquisition::~GalileoE1PcpsCccwsrAmbiguousAcquisition() -{ - delete[] code_data_; - delete[] code_pilot_; -} +GalileoE1PcpsCccwsrAmbiguousAcquisition::~GalileoE1PcpsCccwsrAmbiguousAcquisition() = default; void GalileoE1PcpsCccwsrAmbiguousAcquisition::stop_acquisition() @@ -168,6 +164,7 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_doppler_step(unsigned int dopp } } + void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_gnss_synchro( Gnss_Synchro* gnss_synchro) { @@ -205,15 +202,15 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_local_code() std::array signal = {{'1', 'B', '\0'}}; - galileo_e1_code_gen_complex_sampled(gsl::span(code_data_, vector_length_), signal, + galileo_e1_code_gen_complex_sampled(gsl::span(code_data_.data(), vector_length_), signal, cboc, gnss_synchro_->PRN, fs_in_, 0, false); std::array signal_C = {{'1', 'C', '\0'}}; - galileo_e1_code_gen_complex_sampled(gsl::span(code_pilot_, vector_length_), signal_C, + galileo_e1_code_gen_complex_sampled(gsl::span(code_pilot_.data(), vector_length_), signal_C, cboc, gnss_synchro_->PRN, fs_in_, 0, false); - acquisition_cc_->set_local_code(code_data_, code_pilot_); + acquisition_cc_->set_local_code(code_data_.data(), code_pilot_.data()); } } @@ -226,6 +223,7 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::reset() } } + void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_state(int state) { acquisition_cc_->set_state(state); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.h index b79fe2d33..eac80983c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.h @@ -36,7 +36,9 @@ #include "gnss_synchro.h" #include "pcps_cccwsr_acquisition_cc.h" #include +#include #include +#include class ConfigurationInterface; @@ -94,13 +96,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of CCCWSR algorithm */ @@ -153,7 +156,6 @@ private: std::string item_type_; unsigned int vector_length_; unsigned int code_length_; - //unsigned int satellite_; unsigned int channel_; std::weak_ptr channel_fsm_; float threshold_; @@ -164,8 +166,8 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_data_; - std::complex* code_pilot_; + std::vector> code_data_; + std::vector> code_pilot_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc index 4e4c06ba9..9a3462407 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc @@ -115,7 +115,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); - code_ = new gr_complex[code_length_]; + code_ = std::vector>(code_length_); LOG(INFO) << "Vector Length: " << vector_length_ << ", Samples per ms: " << samples_per_ms << ", Folding factor: " << folding_factor_ @@ -157,10 +157,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui } -GalileoE1PcpsQuickSyncAmbiguousAcquisition::~GalileoE1PcpsQuickSyncAmbiguousAcquisition() -{ - delete[] code_; -} +GalileoE1PcpsQuickSyncAmbiguousAcquisition::~GalileoE1PcpsQuickSyncAmbiguousAcquisition() = default; void GalileoE1PcpsQuickSyncAmbiguousAcquisition::stop_acquisition() @@ -251,23 +248,20 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code() bool cboc = configuration_->property( "Acquisition" + std::to_string(channel_) + ".cboc", false); - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; std::array Signal_; std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); - galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, + galileo_e1_code_gen_complex_sampled(gsl::span>(code.get(), code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < (sampled_ms_ / (folding_factor_ * 4)); i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_cc_->set_local_code(code_); - - delete[] code; - code = nullptr; + acquisition_cc_->set_local_code(code_.data()); } } @@ -280,6 +274,7 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisition::reset() } } + void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_state(int state) { if (item_type_ == "gr_complex") diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.h index c717c4ed7..ecbac95b6 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.h @@ -36,7 +36,9 @@ #include "gnss_synchro.h" #include "pcps_quicksync_acquisition_cc.h" #include +#include #include +#include class ConfigurationInterface; @@ -169,7 +171,7 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc index 4ab468fe8..3bb75b971 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc @@ -90,7 +90,7 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition( int samples_per_ms = code_length_ / 4; - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -127,10 +127,7 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition( } -GalileoE1PcpsTongAmbiguousAcquisition::~GalileoE1PcpsTongAmbiguousAcquisition() -{ - delete[] code_; -} +GalileoE1PcpsTongAmbiguousAcquisition::~GalileoE1PcpsTongAmbiguousAcquisition() = default; void GalileoE1PcpsTongAmbiguousAcquisition::stop_acquisition() @@ -221,21 +218,19 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_local_code() bool cboc = configuration_->property( "Acquisition" + std::to_string(channel_) + ".cboc", false); - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; std::array Signal_; std::memcpy(Signal_.data(), gnss_synchro_->Signal, 3); - galileo_e1_code_gen_complex_sampled(gsl::span>(code, code_length_), Signal_, + galileo_e1_code_gen_complex_sampled(gsl::span>(code.get(), code_length_), Signal_, cboc, gnss_synchro_->PRN, fs_in_, 0, false); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_cc_->set_local_code(code_); - - delete[] code; + acquisition_cc_->set_local_code(code_.data()); } } @@ -248,6 +243,7 @@ void GalileoE1PcpsTongAmbiguousAcquisition::reset() } } + void GalileoE1PcpsTongAmbiguousAcquisition::set_state(int state) { acquisition_cc_->set_state(state); diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.h index 2cb201866..402383fcb 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.h @@ -36,7 +36,9 @@ #include "gnss_synchro.h" #include "pcps_tong_acquisition_cc.h" #include +#include #include +#include class ConfigurationInterface; @@ -168,7 +170,7 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc index e12e893e2..e02e6b97f 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc @@ -94,8 +94,8 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf( vector_length_ = code_length_ * sampled_ms_; - codeI_ = new gr_complex[vector_length_]; - codeQ_ = new gr_complex[vector_length_]; + codeI_ = std::vector>(vector_length_); + codeQ_ = std::vector>(vector_length_); both_signal_components = false; std::string sig_ = configuration_->property("Channel.signal", std::string("5X")); @@ -132,11 +132,7 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf( } -GalileoE5aNoncoherentIQAcquisitionCaf::~GalileoE5aNoncoherentIQAcquisitionCaf() -{ - delete[] codeI_; - delete[] codeQ_; -} +GalileoE5aNoncoherentIQAcquisitionCaf::~GalileoE5aNoncoherentIQAcquisitionCaf() = default; void GalileoE5aNoncoherentIQAcquisitionCaf::stop_acquisition() @@ -224,53 +220,51 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_local_code() { if (item_type_ == "gr_complex") { - auto* codeI = new std::complex[code_length_]; - auto* codeQ = new std::complex[code_length_]; + std::vector> codeI(code_length_); + std::vector> codeQ(code_length_); if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') { std::array a = {{'5', 'I', '\0'}}; - galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeI, code_length_), a, + galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeI.data(), code_length_), a, gnss_synchro_->PRN, fs_in_, 0); std::array b = {{'5', 'Q', '\0'}}; - galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeQ, code_length_), b, + galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeQ.data(), code_length_), b, gnss_synchro_->PRN, fs_in_, 0); } else { std::array signal_type_ = {{'5', 'X', '\0'}}; - galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeI, code_length_), signal_type_, + galileo_e5_a_code_gen_complex_sampled(gsl::span>(codeI.data(), code_length_), signal_type_, gnss_synchro_->PRN, fs_in_, 0); } // WARNING: 3ms are coherently integrated. Secondary sequence (1,1,1) // is generated, and modulated in the 'block'. - gsl::span codeQ_span(codeQ_, vector_length_); - gsl::span codeI_span(codeI_, vector_length_); + gsl::span codeQ_span(codeQ_.data(), vector_length_); + gsl::span codeI_span(codeI_.data(), vector_length_); if (Zero_padding == 0) // if no zero_padding { for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(codeI, code_length_, codeI_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(codeI.data(), code_length_, codeI_span.subspan(i * code_length_, code_length_).data()); if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') { - std::copy_n(codeQ, code_length_, codeQ_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(codeQ.data(), code_length_, codeQ_span.subspan(i * code_length_, code_length_).data()); } } } else { // 1ms code + 1ms zero padding - std::copy_n(codeI, code_length_, codeI_); + std::copy_n(codeI.data(), code_length_, codeI_.data()); if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') { - std::copy_n(codeQ, code_length_, codeQ_); + std::copy_n(codeQ.data(), code_length_, codeQ_.data()); } } - acquisition_cc_->set_local_code(codeI_, codeQ_); - delete[] codeI; - delete[] codeQ; + acquisition_cc_->set_local_code(codeI_.data(), codeQ_.data()); } } diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.h b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.h index aefcd7a23..7064c0aa9 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.h @@ -41,7 +41,9 @@ #include "channel_fsm.h" #include "galileo_e5a_noncoherent_iq_acquisition_caf_cc.h" #include "gnss_synchro.h" +#include #include +#include class ConfigurationInterface; @@ -95,13 +97,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -171,8 +174,8 @@ private: std::string dump_filename_; int Zero_padding; int CAF_window_hz_; - std::complex* codeI_; - std::complex* codeQ_; + std::vector> codeI_; + std::vector> codeQ_; bool both_signal_components; Gnss_Synchro* gnss_synchro_; std::string role_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc index 7da2a1bd6..7f8d72196 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc @@ -124,7 +124,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con code_length_ = static_cast(std::round(static_cast(fs_in_) / GALILEO_E5A_CODE_CHIP_RATE_HZ * static_cast(GALILEO_E5A_CODE_LENGTH_CHIPS))); vector_length_ = code_length_ * sampled_ms_; - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -165,10 +165,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con } -GalileoE5aPcpsAcquisition::~GalileoE5aPcpsAcquisition() -{ - delete[] code_; -} +GalileoE5aPcpsAcquisition::~GalileoE5aPcpsAcquisition() = default; void GalileoE5aPcpsAcquisition::stop_acquisition() @@ -236,7 +233,7 @@ void GalileoE5aPcpsAcquisition::init() void GalileoE5aPcpsAcquisition::set_local_code() { - auto* code = new gr_complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; std::array signal_; signal_[0] = '5'; signal_[2] = '\0'; @@ -256,20 +253,19 @@ void GalileoE5aPcpsAcquisition::set_local_code() if (acq_parameters_.use_automatic_resampler) { - galileo_e5_a_code_gen_complex_sampled(gsl::span(code, code_length_), signal_, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0); + galileo_e5_a_code_gen_complex_sampled(gsl::span(code.get(), code_length_), signal_, gnss_synchro_->PRN, acq_parameters_.resampled_fs, 0); } else { - galileo_e5_a_code_gen_complex_sampled(gsl::span(code, code_length_), signal_, gnss_synchro_->PRN, fs_in_, 0); + galileo_e5_a_code_gen_complex_sampled(gsl::span(code.get(), code_length_), signal_, gnss_synchro_->PRN, fs_in_, 0); } - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h index 832f3223a..f6b694a67 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h @@ -35,7 +35,9 @@ #include "channel_fsm.h" #include "gnss_synchro.h" #include "pcps_acquisition.h" +#include #include +#include class ConfigurationInterface; @@ -86,13 +88,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -143,29 +146,23 @@ public: /*! * \brief Sets the resampler latency to account it in the acquisition code delay estimation */ - void set_resampler_latency(uint32_t latency_samples) override; private: float calculate_threshold(float pfa); - ConfigurationInterface* configuration_; - pcps_acquisition_sptr acquisition_; Acq_Conf acq_parameters_; size_t item_size_; - std::string item_type_; std::string dump_filename_; std::string role_; - bool bit_transition_flag_; bool dump_; bool acq_pilot_; bool use_CFAR_; bool blocking_; bool acq_iq_; - unsigned int vector_length_; unsigned int code_length_; unsigned int channel_; @@ -176,18 +173,10 @@ private: unsigned int max_dwells_; unsigned int in_streams_; unsigned int out_streams_; - int64_t fs_in_; - float threshold_; - - /* - std::complex* codeI_; - std::complex* codeQ_; - */ - - gr_complex* code_; - + std::vector> code_; Gnss_Synchro* gnss_synchro_; }; + #endif /* GALILEO_E5A_PCPS_ACQUISITION_H_ */ diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index 15361509f..54f367158 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -110,9 +110,9 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf // compute all the GALILEO E5 PRN Codes (this is done only once in the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT - auto* code = new std::complex[nsamples_total]; // buffer for the local code + std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new uint32_t[(nsamples_total * GALILEO_E5A_NUMBER_OF_CODES)]; // memory containing all the possible fft codes for PRN 0 to 32 + d_all_fft_codes_ = std::vector(nsamples_total * GALILEO_E5A_NUMBER_OF_CODES); // memory containing all the possible fft codes for PRN 0 to 32 float max; // temporary maxima search int32_t tmp, tmp2, local_code, fft_data; @@ -136,7 +136,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf signal_[1] = 'I'; } - galileo_e5_a_code_gen_complex_sampled(gsl::span>(code, nsamples_total), signal_, PRN, fs_in, 0); + galileo_e5_a_code_gen_complex_sampled(gsl::span>(code.data(), nsamples_total), signal_, PRN, fs_in, 0); for (uint32_t s = code_length; s < 2 * code_length; s++) { @@ -149,7 +149,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf code[s] = std::complex(0.0, 0.0); } - std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer + std::copy_n(code.data(), nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values @@ -177,7 +177,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf } } - acq_parameters.all_fft_codes = d_all_fft_codes_; + acq_parameters.all_fft_codes = d_all_fft_codes_.data(); // reference for the FPGA FFT-IFFT attenuation factor acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 13); @@ -193,16 +193,12 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf gnss_synchro_ = nullptr; // temporary buffers that we can delete - delete[] code; delete fft_if; - delete[] fft_codes_padded; + volk_gnsssdr_free(fft_codes_padded); } -GalileoE5aPcpsAcquisitionFpga::~GalileoE5aPcpsAcquisitionFpga() -{ - delete[] d_all_fft_codes_; -} +GalileoE5aPcpsAcquisitionFpga::~GalileoE5aPcpsAcquisitionFpga() = default; void GalileoE5aPcpsAcquisitionFpga::stop_acquisition() diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index d30868547..37e0bf9dd 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -39,7 +39,9 @@ #include // for lv_16sc_t #include // for size_t #include +#include #include +#include class Gnss_Synchro; class ConfigurationInterface; @@ -99,8 +101,8 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; @@ -167,24 +169,19 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; - std::string item_type_; std::string dump_filename_; std::string role_; - bool acq_pilot_; bool acq_iq_; - uint32_t channel_; std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; unsigned int in_streams_; unsigned int out_streams_; - Gnss_Synchro* gnss_synchro_; - - uint32_t* d_all_fft_codes_; // memory that contains all the code ffts + std::vector d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GALILEO_E5A_PCPS_ACQUISITION_FPGA_H_ */ diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc index 45c7645bb..bdac10535 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -94,7 +94,7 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( vector_length_ *= 2; } - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -138,16 +138,14 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( } -GlonassL1CaPcpsAcquisition::~GlonassL1CaPcpsAcquisition() -{ - delete[] code_; -} +GlonassL1CaPcpsAcquisition::~GlonassL1CaPcpsAcquisition() = default; void GlonassL1CaPcpsAcquisition::stop_acquisition() { } + void GlonassL1CaPcpsAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + ".pfa", 0.0); @@ -207,18 +205,17 @@ void GlonassL1CaPcpsAcquisition::init() void GlonassL1CaPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; glonass_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), /* gnss_synchro_->PRN,*/ fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h index a9a6fd18a..1768c3bb4 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h @@ -39,7 +39,9 @@ #include "gnss_synchro.h" #include "pcps_acquisition.h" #include +#include #include +#include class ConfigurationInterface; @@ -86,6 +88,7 @@ public: * tracking blocks */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + /*! * \brief Set acquisition channel unique ID */ @@ -96,13 +99,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -172,12 +176,11 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc index 217f31d31..2210eebbf 100644 --- a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc @@ -93,7 +93,7 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition( vector_length_ *= 2; } - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -137,10 +137,7 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition( } -GlonassL2CaPcpsAcquisition::~GlonassL2CaPcpsAcquisition() -{ - delete[] code_; -} +GlonassL2CaPcpsAcquisition::~GlonassL2CaPcpsAcquisition() = default; void GlonassL2CaPcpsAcquisition::stop_acquisition() @@ -207,18 +204,17 @@ void GlonassL2CaPcpsAcquisition::init() void GlonassL2CaPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; glonass_l2_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), /* gnss_synchro_->PRN,*/ fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.h b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.h index 09b66fbe7..efe36e7ea 100644 --- a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.h @@ -38,7 +38,9 @@ #include "gnss_synchro.h" #include "pcps_acquisition.h" #include +#include #include +#include class ConfigurationInterface; @@ -96,13 +98,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -172,12 +175,11 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc index 297464005..9c50bb3c1 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc @@ -123,7 +123,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition( acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(GPS_L1_CA_CODE_PERIOD * 1000.0); vector_length_ = std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2 : 1); - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -161,16 +161,14 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition( } -GpsL1CaPcpsAcquisition::~GpsL1CaPcpsAcquisition() -{ - delete[] code_; -} +GpsL1CaPcpsAcquisition::~GpsL1CaPcpsAcquisition() = default; void GpsL1CaPcpsAcquisition::stop_acquisition() { } + void GpsL1CaPcpsAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + ".pfa", 0.0); @@ -228,7 +226,7 @@ void GpsL1CaPcpsAcquisition::init() void GpsL1CaPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; if (acq_parameters_.use_automatic_resampler) { @@ -238,14 +236,13 @@ void GpsL1CaPcpsAcquisition::set_local_code() { gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); } - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h index 8d3f0fb64..bef0202bc 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h @@ -43,7 +43,9 @@ #include "pcps_acquisition.h" #include #include +#include #include +#include class ConfigurationInterface; @@ -102,13 +104,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -157,10 +160,8 @@ public: /*! * \brief Sets the resampler latency to account it in the acquisition code delay estimation */ - void set_resampler_latency(uint32_t latency_samples) override; - private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; @@ -184,12 +185,11 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc index 6a4cfa7fd..1c67657b8 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc @@ -80,7 +80,7 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler( //--- Find number of samples per spreading code ------------------------- vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); acq_parameters.samples_per_ms = vector_length_; - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -109,10 +109,7 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler( } -GpsL1CaPcpsAcquisitionFineDoppler::~GpsL1CaPcpsAcquisitionFineDoppler() -{ - delete[] code_; -} +GpsL1CaPcpsAcquisitionFineDoppler::~GpsL1CaPcpsAcquisitionFineDoppler() = default; void GpsL1CaPcpsAcquisitionFineDoppler::stop_acquisition() @@ -163,8 +160,8 @@ void GpsL1CaPcpsAcquisitionFineDoppler::init() void GpsL1CaPcpsAcquisitionFineDoppler::set_local_code() { - gps_l1_ca_code_gen_complex_sampled(gsl::span>(code_, vector_length_), gnss_synchro_->PRN, fs_in_, 0); - acquisition_cc_->set_local_code(code_); + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code_.data(), vector_length_), gnss_synchro_->PRN, fs_in_, 0); + acquisition_cc_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h index d310bcfbb..80fc080b6 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h @@ -37,7 +37,9 @@ #include "channel_fsm.h" #include "gnss_synchro.h" #include "pcps_acquisition_fine_doppler_cc.h" +#include #include +#include class ConfigurationInterface; @@ -95,13 +97,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -161,7 +164,7 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index 7c6d7ad2d..a1307b68f 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -105,15 +105,15 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT // allocate memory to compute all the PRNs and compute all the possible codes - auto* code = new std::complex[nsamples_total]; // buffer for the local code + std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new uint32_t[(nsamples_total * NUM_PRNs)]; // memory containing all the possible fft codes for PRN 0 to 32 + d_all_fft_codes_ = std::vector(nsamples_total * NUM_PRNs); // memory containing all the possible fft codes for PRN 0 to 32 float max; int32_t tmp, tmp2, local_code, fft_data; // temporary maxima search for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, nsamples_total), PRN, fs_in, 0); // generate PRN code + gps_l1_ca_code_gen_complex_sampled(gsl::span>(code.data(), nsamples_total), PRN, fs_in, 0); // generate PRN code for (uint32_t s = code_length; s < 2 * code_length; s++) { @@ -126,7 +126,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( code[s] = std::complex(0.0, 0.0); } - std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer + std::copy_n(code.data(), nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values @@ -155,7 +155,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( } //acq_parameters - acq_parameters.all_fft_codes = d_all_fft_codes_; + acq_parameters.all_fft_codes = d_all_fft_codes_.data(); // reference for the FPGA FFT-IFFT attenuation factor acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 10); @@ -171,16 +171,12 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( gnss_synchro_ = nullptr; // temporary buffers that we can delete - delete[] code; delete fft_if; - delete[] fft_codes_padded; + volk_gnsssdr_free(fft_codes_padded); } -GpsL1CaPcpsAcquisitionFpga::~GpsL1CaPcpsAcquisitionFpga() -{ - delete[] d_all_fft_codes_; -} +GpsL1CaPcpsAcquisitionFpga::~GpsL1CaPcpsAcquisitionFpga() = default; void GpsL1CaPcpsAcquisitionFpga::stop_acquisition() diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index f0f652a99..39b8d2e1f 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -41,7 +41,9 @@ #include // for basic_block_sptr, top_block_sptr #include // for lv_16sc_t #include // for size_t -#include // for string +#include +#include +#include class Gnss_Synchro; class ConfigurationInterface; @@ -167,7 +169,7 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - uint32_t* d_all_fft_codes_; // memory that contains all the code ffts + std::vector d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GPS_L1_CA_PCPS_ACQUISITION_FPGA_H_ */ diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc index d603eb216..592648bd6 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc @@ -70,7 +70,7 @@ GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition( //--- Find number of samples per spreading code ------------------------- vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); - code_ = new gr_complex[vector_length_]; + code_ = std::make_shared>(vector_length_); if (item_type_ == "gr_complex") { @@ -101,10 +101,7 @@ GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition( } -GpsL1CaPcpsAssistedAcquisition::~GpsL1CaPcpsAssistedAcquisition() -{ - delete[] code_; -} +GpsL1CaPcpsAssistedAcquisition::~GpsL1CaPcpsAssistedAcquisition() = default; void GpsL1CaPcpsAssistedAcquisition::stop_acquisition() @@ -154,8 +151,8 @@ void GpsL1CaPcpsAssistedAcquisition::init() void GpsL1CaPcpsAssistedAcquisition::set_local_code() { - gps_l1_ca_code_gen_complex_sampled(gsl::span(code_, vector_length_), gnss_synchro_->PRN, fs_in_, 0); - acquisition_cc_->set_local_code(code_); + gps_l1_ca_code_gen_complex_sampled(gsl::span(code_.get(), vector_length_), gnss_synchro_->PRN, fs_in_, 0); + acquisition_cc_->set_local_code(code_.get()); } void GpsL1CaPcpsAssistedAcquisition::reset() diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h index 222bfafe8..9b38e6608 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h @@ -37,7 +37,9 @@ #include "channel_fsm.h" #include "gnss_synchro.h" #include "pcps_assisted_acquisition_cc.h" +#include #include +#include class ConfigurationInterface; @@ -95,13 +97,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -147,7 +150,6 @@ private: size_t item_size_; std::string item_type_; unsigned int vector_length_; - //unsigned int satellite_; unsigned int channel_; std::weak_ptr channel_fsm_; float threshold_; @@ -159,7 +161,7 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::shared_ptr> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc index f5d193d13..de5832cf1 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc @@ -85,7 +85,7 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition( vector_length_ = code_length_ * sampled_ms_; - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -121,16 +121,14 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition( } -GpsL1CaPcpsOpenClAcquisition::~GpsL1CaPcpsOpenClAcquisition() -{ - delete[] code_; -} +GpsL1CaPcpsOpenClAcquisition::~GpsL1CaPcpsOpenClAcquisition() = default; void GpsL1CaPcpsOpenClAcquisition::stop_acquisition() { } + void GpsL1CaPcpsOpenClAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); @@ -211,19 +209,17 @@ void GpsL1CaPcpsOpenClAcquisition::set_local_code() { if (item_type_ == "gr_complex") { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_cc_->set_local_code(code_); - - delete[] code; + acquisition_cc_->set_local_code(code_.data()); } } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.h index 350589f17..c70d77a54 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.h @@ -36,7 +36,9 @@ #include "gnss_synchro.h" #include "pcps_opencl_acquisition_cc.h" #include +#include #include +#include class ConfigurationInterface; @@ -94,13 +96,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -169,12 +172,11 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc index 2867b53a0..251216eab 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc @@ -105,7 +105,7 @@ GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition( dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); int samples_per_ms = round(code_length_); - code_ = new gr_complex[code_length_](); + code_ = std::vector>(code_length_); /*Object relevant information for debugging*/ LOG(INFO) << "Implementation: " << this->implementation() << ", Vector Length: " << vector_length_ @@ -150,16 +150,14 @@ GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition( } -GpsL1CaPcpsQuickSyncAcquisition::~GpsL1CaPcpsQuickSyncAcquisition() -{ - delete[] code_; -} +GpsL1CaPcpsQuickSyncAcquisition::~GpsL1CaPcpsQuickSyncAcquisition() = default; void GpsL1CaPcpsQuickSyncAcquisition::stop_acquisition() { } + void GpsL1CaPcpsQuickSyncAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); @@ -237,19 +235,17 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_local_code() { if (item_type_ == "gr_complex") { - auto* code = new std::complex[code_length_](); + std::unique_ptr> code{new std::complex[code_length_]}; gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < (sampled_ms_ / folding_factor_); i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_cc_->set_local_code(code_); - - delete[] code; + acquisition_cc_->set_local_code(code_.data()); } } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.h index 110589952..7f5390782 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.h @@ -38,7 +38,9 @@ #include "gnss_synchro.h" #include "pcps_quicksync_acquisition_cc.h" #include +#include #include +#include class ConfigurationInterface; @@ -96,13 +98,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -170,12 +173,11 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc index 731bd2e77..c670cc0bb 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc @@ -76,7 +76,7 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition( vector_length_ = code_length_ * sampled_ms_; - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "gr_complex") { @@ -112,16 +112,14 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition( } -GpsL1CaPcpsTongAcquisition::~GpsL1CaPcpsTongAcquisition() -{ - delete[] code_; -} +GpsL1CaPcpsTongAcquisition::~GpsL1CaPcpsTongAcquisition() = default; void GpsL1CaPcpsTongAcquisition::stop_acquisition() { } + void GpsL1CaPcpsTongAcquisition::set_threshold(float threshold) { float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); @@ -194,23 +192,22 @@ void GpsL1CaPcpsTongAcquisition::init() //set_local_code(); } + void GpsL1CaPcpsTongAcquisition::set_local_code() { if (item_type_ == "gr_complex") { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; gps_l1_ca_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < sampled_ms_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_cc_->set_local_code(code_); - - delete[] code; + acquisition_cc_->set_local_code(code_.data()); } } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.h index 856b1c603..7193e5466 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.h @@ -37,7 +37,9 @@ #include "gnss_synchro.h" #include "pcps_tong_acquisition_cc.h" #include +#include #include +#include class ConfigurationInterface; @@ -95,13 +97,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_cc_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of TONG algorithm */ @@ -169,12 +172,11 @@ private: int64_t fs_in_; bool dump_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc index 2777d8440..d424bdb5f 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc @@ -127,7 +127,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition( acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(GPS_L2_M_PERIOD * 1000.0); vector_length_ = acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms * (acq_parameters_.bit_transition_flag ? 2 : 1); - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); if (item_type_ == "cshort") { @@ -165,10 +165,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition( } -GpsL2MPcpsAcquisition::~GpsL2MPcpsAcquisition() -{ - delete[] code_; -} +GpsL2MPcpsAcquisition::~GpsL2MPcpsAcquisition() = default; void GpsL2MPcpsAcquisition::stop_acquisition() @@ -240,25 +237,24 @@ void GpsL2MPcpsAcquisition::init() void GpsL2MPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; if (acq_parameters_.use_automatic_resampler) { - gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); + gps_l2c_m_code_gen_complex_sampled(gsl::span>(code.get(), code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); } else { - gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_); + gps_l2c_m_code_gen_complex_sampled(gsl::span>(code.get(), code_length_), gnss_synchro_->PRN, fs_in_); } - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < num_codes_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h index d189a3032..25a792d69 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h @@ -40,7 +40,9 @@ #include "pcps_acquisition.h" #include #include +#include #include +#include class ConfigurationInterface; @@ -99,13 +101,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -154,10 +157,8 @@ public: /*! * \brief Sets the resampler latency to account it in the acquisition code delay estimation */ - void set_resampler_latency(uint32_t latency_samples) override; - private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; @@ -180,13 +181,12 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; unsigned int num_codes_; - float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index dcbbd331a..a92f5c158 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -104,22 +104,21 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(vector_length, true); // Direct FFT // allocate memory to compute all the PRNs and compute all the possible codes - auto* code = new std::complex[nsamples_total]; // buffer for the local code + std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - //d_all_fft_codes_ = new lv_16sc_t[nsamples_total * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32 - d_all_fft_codes_ = new uint32_t[(nsamples_total * NUM_PRNs)]; // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + d_all_fft_codes_ = std::vector(nsamples_total * NUM_PRNs); // memory containing all the possible fft codes for PRN 0 to 32 + float max; // temporary maxima search int32_t tmp, tmp2, local_code, fft_data; for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l2c_m_code_gen_complex_sampled(gsl::span>(code, nsamples_total), PRN, fs_in_); + gps_l2c_m_code_gen_complex_sampled(gsl::span>(code.data(), nsamples_total), PRN, fs_in_); // fill in zero padding for (unsigned int s = code_length; s < nsamples_total; s++) { code[s] = std::complex(0.0, 0.0); } - std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer + std::copy_n(code.data(), nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values max = 0; // initialize maximum value @@ -149,12 +148,11 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( } } - acq_parameters.all_fft_codes = d_all_fft_codes_; + acq_parameters.all_fft_codes = d_all_fft_codes_.data(); // temporary buffers that we can delete - delete[] code; delete fft_if; - delete[] fft_codes_padded; + volk_gnsssdr_free(fft_codes_padded); acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); @@ -162,15 +160,11 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( doppler_step_ = 0; gnss_synchro_ = nullptr; - threshold_ = 0.0; } -GpsL2MPcpsAcquisitionFpga::~GpsL2MPcpsAcquisitionFpga() -{ - delete[] d_all_fft_codes_; -} +GpsL2MPcpsAcquisitionFpga::~GpsL2MPcpsAcquisitionFpga() = default; void GpsL2MPcpsAcquisitionFpga::stop_acquisition() diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h index 7c30efb85..9507abffd 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h @@ -39,7 +39,9 @@ #include // for basic_block_sptr, top_block_sptr #include // for lv_16sc_t #include // for size_t +#include // for weak_ptr #include // for string +#include class Gnss_Synchro; class ConfigurationInterface; @@ -98,8 +100,8 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; @@ -168,10 +170,7 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - - uint32_t* d_all_fft_codes_; // memory that contains all the code ffts - - //float calculate_threshold(float pfa); + std::vector d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GPS_L2_M_PCPS_ACQUISITION_FPGA_H_ */ diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc index 3c30f8e5f..8b0a6e793 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc @@ -134,7 +134,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(GPS_L5I_PERIOD * 1000.0); vector_length_ = std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2 : 1); - code_ = new gr_complex[vector_length_]; + code_ = std::vector>(vector_length_); acquisition_ = pcps_make_acquisition(acq_parameters_); DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; @@ -160,10 +160,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( } -GpsL5iPcpsAcquisition::~GpsL5iPcpsAcquisition() -{ - delete[] code_; -} +GpsL5iPcpsAcquisition::~GpsL5iPcpsAcquisition() = default; void GpsL5iPcpsAcquisition::stop_acquisition() @@ -234,25 +231,24 @@ void GpsL5iPcpsAcquisition::init() void GpsL5iPcpsAcquisition::set_local_code() { - auto* code = new std::complex[code_length_]; + std::unique_ptr> code{new std::complex[code_length_]}; if (acq_parameters_.use_automatic_resampler) { - gps_l5i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); + gps_l5i_code_gen_complex_sampled(gsl::span>(code.get(), code_length_), gnss_synchro_->PRN, acq_parameters_.resampled_fs); } else { - gps_l5i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_); + gps_l5i_code_gen_complex_sampled(gsl::span>(code.get(), code_length_), gnss_synchro_->PRN, fs_in_); } - gsl::span code_span(code_, vector_length_); + gsl::span code_span(code_.data(), vector_length_); for (unsigned int i = 0; i < num_codes_; i++) { - std::copy_n(code, code_length_, code_span.subspan(i * code_length_, code_length_).data()); + std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } - acquisition_->set_local_code(code_); - delete[] code; + acquisition_->set_local_code(code_.data()); } @@ -261,6 +257,7 @@ void GpsL5iPcpsAcquisition::reset() acquisition_->set_active(true); } + void GpsL5iPcpsAcquisition::set_state(int state) { acquisition_->set_state(state); @@ -360,6 +357,7 @@ gr::basic_block_sptr GpsL5iPcpsAcquisition::get_right_block() return acquisition_; } + void GpsL5iPcpsAcquisition::set_resampler_latency(uint32_t latency_samples) { acquisition_->set_resampler_latency(latency_samples); diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h index 576c86e33..17a1b634c 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h @@ -31,8 +31,8 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_ -#define GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_ +#ifndef GNSS_SDR_GPS_L5I_PCPS_ACQUISITION_H_ +#define GNSS_SDR_GPS_L5I_PCPS_ACQUISITION_H_ #include "channel_fsm.h" #include "complex_byte_to_float_x2.h" @@ -40,7 +40,9 @@ #include "pcps_acquisition.h" #include #include +#include #include +#include class ConfigurationInterface; @@ -99,13 +101,14 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; acquisition_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -154,7 +157,6 @@ public: /*! * \brief Sets the resampler latency to account it in the acquisition code delay estimation */ - void set_resampler_latency(uint32_t latency_samples) override; private: @@ -179,14 +181,13 @@ private: bool dump_; bool blocking_; std::string dump_filename_; - std::complex* code_; + std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int num_codes_; unsigned int in_streams_; unsigned int out_streams_; - float calculate_threshold(float pfa); }; -#endif /* GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_ */ +#endif /* GNSS_SDR_GPS_L5I_PCPS_ACQUISITION_H_ */ diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index 118fdf651..c25f65672 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -109,16 +109,16 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( // compute all the GPS L5 PRN Codes (this is done only once upon the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT - auto* code = new gr_complex[nsamples_total]; + std::vector> code(nsamples_total); auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new uint32_t[(nsamples_total * NUM_PRNs)]; // memory containing all the possible fft codes for PRN 0 to 32 + d_all_fft_codes_ = std::vector(nsamples_total * NUM_PRNs); // memory containing all the possible fft codes for PRN 0 to 32 float max; // temporary maxima search int32_t tmp, tmp2, local_code, fft_data; for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { - gps_l5i_code_gen_complex_sampled(gsl::span(code, nsamples_total), PRN, fs_in); + gps_l5i_code_gen_complex_sampled(gsl::span(code.data(), nsamples_total), PRN, fs_in); for (uint32_t s = code_length; s < 2 * code_length; s++) { @@ -130,7 +130,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( // fill in zero padding code[s] = std::complex(0.0, 0.0); } - std::copy_n(code, nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer + std::copy_n(code.data(), nsamples_total, fft_if->get_inbuf()); // copy to FFT buffer fft_if->execute(); // Run the FFT of local code volk_32fc_conjugate_32fc(fft_codes_padded, fft_if->get_outbuf(), nsamples_total); // conjugate values @@ -158,7 +158,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( } } - acq_parameters.all_fft_codes = d_all_fft_codes_; + acq_parameters.all_fft_codes = d_all_fft_codes_.data(); // reference for the FPGA FFT-IFFT attenuation factor acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 13); @@ -174,17 +174,12 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( gnss_synchro_ = nullptr; // temporary buffers that we can delete - delete[] code; delete fft_if; - delete[] fft_codes_padded; + volk_gnsssdr_free(fft_codes_padded); } -GpsL5iPcpsAcquisitionFpga::~GpsL5iPcpsAcquisitionFpga() -{ - //delete[] code_; - delete[] d_all_fft_codes_; -} +GpsL5iPcpsAcquisitionFpga::~GpsL5iPcpsAcquisitionFpga() = default; void GpsL5iPcpsAcquisitionFpga::stop_acquisition() diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index ebcbe5f31..1d5c8e922 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -40,7 +40,9 @@ #include // for basic_block_sptr, top_block_sptr #include // for lv_16sc_t #include // for size_t +#include #include +#include class Gnss_Synchro; class ConfigurationInterface; @@ -99,8 +101,8 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::weak_ptr channel_fsm) override { channel_fsm_ = channel_fsm; @@ -167,9 +169,7 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - - uint32_t* d_all_fft_codes_; // memory that contains all the code ffts - + std::vector d_all_fft_codes_; // memory that contains all the code ffts float calculate_threshold(float pfa); }; diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index 1bf97f3b2..5bed837ee 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -34,6 +34,7 @@ #include "Galileo_E1.h" #include "gnss_signal_processing.h" #include +#include #include @@ -171,10 +172,8 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< galileo_e1_code_gen_int(gsl::span(primary_code_E1_chips, static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS)), _Signal, _prn); // generate Galileo E1 code, 1 sample per chip - float* _signal_E1; - _codeLength = _samplesPerChip * GALILEO_E1_B_CODE_LENGTH_CHIPS; - _signal_E1 = new float[_codeLength]; + std::unique_ptr _signal_E1{new float[_codeLength]}; gsl::span _signal_E1_span(_signal_E1, _codeLength); if (_cboc == true) @@ -196,12 +195,11 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< if (_fs != _samplesPerChip * _codeFreqBasis) { - auto* _resampled_signal = new float[_samplesPerCode]; + std::unique_ptr _resampled_signal{new float[_samplesPerCode]}; resampler(gsl::span(_signal_E1, _codeLength), gsl::span(_resampled_signal, _samplesPerCode), _samplesPerChip * _codeFreqBasis, _fs); // resamples code to fs - delete[] _signal_E1; - _signal_E1 = _resampled_signal; + _signal_E1 = std::move(_resampled_signal); } uint32_t size_signal_E1 = _codeLength; if (_fs != _samplesPerChip * _codeFreqBasis) @@ -211,7 +209,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< gsl::span _signal_E1_span_aux(_signal_E1, size_signal_E1); if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2 && _secondary_flag) { - auto* _signal_E1C_secondary = new float[static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode]; + std::unique_ptr _signal_E1C_secondary{new float[static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode]}; gsl::span _signal_E1C_secondary_span(_signal_E1C_secondary, static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH) * _samplesPerCode); for (uint32_t i = 0; i < static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH); i++) { @@ -223,8 +221,7 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< _samplesPerCode *= static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH); - delete[] _signal_E1; - _signal_E1 = _signal_E1C_secondary_span.data(); + _signal_E1 = std::move(_signal_E1C_secondary); } if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2 && _secondary_flag) { @@ -236,7 +233,6 @@ void galileo_e1_code_gen_float_sampled(gsl::span _dest, const std::array< _dest[(i + delay) % _samplesPerCode] = _signal_E1_span_aux2[i]; } - delete[] _signal_E1; volk_gnsssdr_free(primary_code_E1_chips); } diff --git a/src/algorithms/libs/galileo_e5_signal_processing.cc b/src/algorithms/libs/galileo_e5_signal_processing.cc index 38a6e79e3..070e8c756 100644 --- a/src/algorithms/libs/galileo_e5_signal_processing.cc +++ b/src/algorithms/libs/galileo_e5_signal_processing.cc @@ -35,6 +35,7 @@ #include "Galileo_E5a.h" #include "gnss_signal_processing.h" #include +#include void galileo_e5_a_code_gen_complex_primary(gsl::span> _dest, int32_t _prn, const std::array& _Signal) @@ -108,7 +109,7 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, const uint32_t _codeLength = GALILEO_E5A_CODE_LENGTH_CHIPS; const int32_t _codeFreqBasis = GALILEO_E5A_CODE_CHIP_RATE_HZ; - auto* _code = new std::complex[_codeLength](); + std::unique_ptr> _code{new std::complex[_codeLength]}; gsl::span> _code_span(_code, _codeLength); galileo_e5_a_code_gen_complex_primary(_code_span, _prn, _Signal); @@ -118,13 +119,9 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, if (_fs != _codeFreqBasis) { - std::complex* _resampled_signal; - if (posix_memalign(reinterpret_cast(&_resampled_signal), 16, _samplesPerCode * sizeof(gr_complex)) == 0) - { - }; + std::unique_ptr> _resampled_signal{new std::complex[_samplesPerCode]}; resampler(_code_span, gsl::span>(_resampled_signal, _samplesPerCode), _codeFreqBasis, _fs); // resamples code to fs - delete[] _code; - _code = _resampled_signal; + _code = std::move(_resampled_signal); } uint32_t size_code = _codeLength; if (_fs != _codeFreqBasis) @@ -136,12 +133,4 @@ void galileo_e5_a_code_gen_complex_sampled(gsl::span> _dest, { _dest[(i + delay) % _samplesPerCode] = _code_span_aux[i]; } - if (_fs != _codeFreqBasis) - { - free(_code); - } - else - { - delete[] _code; - } } diff --git a/src/algorithms/libs/gps_l2c_signal.cc b/src/algorithms/libs/gps_l2c_signal.cc index 2708926f7..cfb49082e 100644 --- a/src/algorithms/libs/gps_l2c_signal.cc +++ b/src/algorithms/libs/gps_l2c_signal.cc @@ -33,6 +33,7 @@ #include "gps_l2c_signal.h" #include "GPS_L2C.h" #include +#include int32_t gps_l2c_m_shift(int32_t x) @@ -55,7 +56,7 @@ void gps_l2c_m_code(gsl::span _dest, uint32_t _prn) void gps_l2c_m_code_gen_complex(gsl::span> _dest, uint32_t _prn) { - auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L2_M_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -66,14 +67,12 @@ void gps_l2c_m_code_gen_complex(gsl::span> _dest, uint32_t _ { _dest[i] = std::complex(1.0 - 2.0 * _code_span[i], 0.0); } - - delete[] _code; } void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn) { - auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L2_M_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -84,8 +83,6 @@ void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn) { _dest[i] = 1.0 - 2.0 * static_cast(_code_span[i]); } - - delete[] _code; } @@ -94,7 +91,7 @@ void gps_l2c_m_code_gen_float(gsl::span _dest, uint32_t _prn) */ void gps_l2c_m_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { - auto* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L2_M_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -131,5 +128,4 @@ void gps_l2c_m_code_gen_complex_sampled(gsl::span> _dest, ui _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeValueIndex], 0); //repeat the chip -> upsample } } - delete[] _code; } diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index 9a9b89e03..8fa132b63 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -173,7 +173,7 @@ void make_l5q(gsl::span _dest, int32_t prn) void gps_l5i_code_gen_complex(gsl::span> _dest, uint32_t _prn) { - auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L5I_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -184,14 +184,12 @@ void gps_l5i_code_gen_complex(gsl::span> _dest, uint32_t _pr { _dest[i] = std::complex(1.0 - 2.0 * _code_span[i], 0.0); } - - delete[] _code; } void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn) { - auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L5I_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -202,8 +200,6 @@ void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn) { _dest[i] = 1.0 - 2.0 * static_cast(_code_span[i]); } - - delete[] _code; } @@ -212,7 +208,7 @@ void gps_l5i_code_gen_float(gsl::span _dest, uint32_t _prn) */ void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { - auto* _code = new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L5I_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L5I_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -249,13 +245,12 @@ void gps_l5i_code_gen_complex_sampled(gsl::span> _dest, uint _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeValueIndex], 0.0); // repeat the chip -> upsample } } - delete[] _code; } void gps_l5q_code_gen_complex(gsl::span> _dest, uint32_t _prn) { - auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L5Q_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -266,14 +261,12 @@ void gps_l5q_code_gen_complex(gsl::span> _dest, uint32_t _pr { _dest[i] = std::complex(1.0 - 2.0 * _code_span[i], 0.0); } - - delete[] _code; } void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn) { - auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L5Q_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -284,8 +277,6 @@ void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn) { _dest[i] = 1.0 - 2.0 * static_cast(_code_span[i]); } - - delete[] _code; } @@ -294,7 +285,7 @@ void gps_l5q_code_gen_float(gsl::span _dest, uint32_t _prn) */ void gps_l5q_code_gen_complex_sampled(gsl::span> _dest, uint32_t _prn, int32_t _fs) { - auto* _code = new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]; + std::unique_ptr _code{new int32_t[GPS_L5Q_CODE_LENGTH_CHIPS]}; gsl::span _code_span(_code, GPS_L5Q_CODE_LENGTH_CHIPS); if (_prn > 0 and _prn < 51) { @@ -332,5 +323,4 @@ void gps_l5q_code_gen_complex_sampled(gsl::span> _dest, uint _dest[i] = std::complex(1.0 - 2.0 * _code_span[_codeValueIndex], 0); // repeat the chip -> upsample } } - delete[] _code; } From 5e427956779e3302db85f77c6cab9ba282a06a91 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 12:09:11 +0200 Subject: [PATCH 20/64] Use auto --- .../adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc | 4 ++-- .../tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc | 4 ++-- .../tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index 83d602ab1..447e22231 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -237,7 +237,7 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++) { - int32_t tmp_value = static_cast(ca_codes_f[s]); + auto tmp_value = static_cast(ca_codes_f[s]); if (tmp_value < 0) { tmp_value = 0; @@ -260,7 +260,7 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++) { - int32_t tmp_value = static_cast(ca_codes_f[s]); + auto tmp_value = static_cast(ca_codes_f[s]); if (tmp_value < 0) { tmp_value = 0; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 37a0fb66e..7e3874101 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -225,7 +225,7 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - int32_t tmp_value = static_cast(aux_code[s].imag()); + auto tmp_value = static_cast(aux_code[s].imag()); if (tmp_value < 0) { tmp_value = 0; @@ -247,7 +247,7 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - int32_t tmp_value = static_cast(aux_code[s].real()); + auto tmp_value = static_cast(aux_code[s].real()); if (tmp_value < 0) { tmp_value = 0; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index f212bbb0a..c034017a8 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -242,7 +242,7 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - int32_t tmp_value = static_cast(tracking_code[s]); + auto tmp_value = static_cast(tracking_code[s]); if (tmp_value < 0) { tmp_value = 0; @@ -266,7 +266,7 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - int32_t tmp_value = static_cast(tracking_code[s]); + auto tmp_value = static_cast(tracking_code[s]); if (tmp_value < 0) { tmp_value = 0; From 2cc06c8b632be3119c0b5a394830de870f94f06d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 12:10:14 +0200 Subject: [PATCH 21/64] Remove delete in FPGA adapters --- .../galileo_e1_pcps_ambiguous_acquisition_fpga.cc | 11 +++++------ .../adapters/galileo_e5a_pcps_acquisition_fpga.cc | 7 +++---- .../adapters/gps_l1_ca_pcps_acquisition_fpga.cc | 7 +++---- .../adapters/gps_l2_m_pcps_acquisition_fpga.cc | 12 ++++++------ .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 5 ++--- 5 files changed, 19 insertions(+), 23 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index 0742b0ff5..e1021b6b7 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -108,12 +108,12 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( // compute all the GALILEO E1 PRN Codes (this is done only once in the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) - auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT - //auto code = std::make_shared>(nsamples_total); // buffer for the local code - std::vector> code(nsamples_total); + auto fft_if = std::unique_ptr(new gr::fft::fft_complex(nsamples_total, true)); // Direct FFT + std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_all_fft_codes_ = std::vector(nsamples_total * GALILEO_E1_NUMBER_OF_CODES); // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + + float max; // temporary maxima search int32_t tmp, tmp2, local_code, fft_data; for (uint32_t PRN = 1; PRN <= GALILEO_E1_NUMBER_OF_CODES; PRN++) @@ -189,8 +189,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( doppler_step_ = 0; gnss_synchro_ = nullptr; - // temporary buffers that we can delete - delete fft_if; + // temporary buffers that we can release volk_gnsssdr_free(fft_codes_padded); } diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index 54f367158..a0bd49ba0 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -109,8 +109,8 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf // compute all the GALILEO E5 PRN Codes (this is done only once in the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) - auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT - std::vector> code(nsamples_total); // buffer for the local code + auto fft_if = std::unique_ptr(new gr::fft::fft_complex(nsamples_total, true)); // Direct FFT + std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_all_fft_codes_ = std::vector(nsamples_total * GALILEO_E5A_NUMBER_OF_CODES); // memory containing all the possible fft codes for PRN 0 to 32 @@ -192,8 +192,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf doppler_step_ = 0; gnss_synchro_ = nullptr; - // temporary buffers that we can delete - delete fft_if; + // temporary buffers that we can release volk_gnsssdr_free(fft_codes_padded); } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index a1307b68f..7216d09d5 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -103,7 +103,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // compute all the GPS L1 PRN Codes (this is done only once upon the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) - auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT + auto fft_if = std::unique_ptr(new gr::fft::fft_complex(nsamples_total, true)); // allocate memory to compute all the PRNs and compute all the possible codes std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); @@ -154,7 +154,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( } } - //acq_parameters + // acq_parameters acq_parameters.all_fft_codes = d_all_fft_codes_.data(); // reference for the FPGA FFT-IFFT attenuation factor @@ -170,8 +170,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( doppler_step_ = 0; gnss_synchro_ = nullptr; - // temporary buffers that we can delete - delete fft_if; + // temporary buffers that we can release volk_gnsssdr_free(fft_codes_padded); } diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index a92f5c158..17d98fc25 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -102,12 +102,13 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( // compute all the GPS L2C PRN Codes (this is done only once upon the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) - auto* fft_if = new gr::fft::fft_complex(vector_length, true); // Direct FFT + auto fft_if = std::unique_ptr(new gr::fft::fft_complex(nsamples_total, true)); // Direct FFT // allocate memory to compute all the PRNs and compute all the possible codes std::vector> code(nsamples_total); // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_all_fft_codes_ = std::vector(nsamples_total * NUM_PRNs); // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + + float max; // temporary maxima search int32_t tmp, tmp2, local_code, fft_data; for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) @@ -150,10 +151,6 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( acq_parameters.all_fft_codes = d_all_fft_codes_.data(); - // temporary buffers that we can delete - delete fft_if; - volk_gnsssdr_free(fft_codes_padded); - acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); channel_ = 0; @@ -161,6 +158,9 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( gnss_synchro_ = nullptr; threshold_ = 0.0; + + // temporary buffers that we can release + volk_gnsssdr_free(fft_codes_padded); } diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index c25f65672..30bd9d407 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -108,7 +108,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( // compute all the GPS L5 PRN Codes (this is done only once upon the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) - auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT + auto fft_if = std::unique_ptr(new gr::fft::fft_complex(nsamples_total, true)); // Direct FFT std::vector> code(nsamples_total); auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_all_fft_codes_ = std::vector(nsamples_total * NUM_PRNs); // memory containing all the possible fft codes for PRN 0 to 32 @@ -173,8 +173,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( doppler_step_ = 0; gnss_synchro_ = nullptr; - // temporary buffers that we can delete - delete fft_if; + // temporary buffers that we can release volk_gnsssdr_free(fft_codes_padded); } From d72542861f4a6666966fca6ec29e4b9ee04be70a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 12:51:23 +0200 Subject: [PATCH 22/64] Replace raw pointers by smart pointers. Remove delete --- .../galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc | 7 ++----- .../galileo_e5a_noncoherent_iq_acquisition_caf_cc.h | 5 +++-- .../gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc | 7 ++----- .../gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h | 5 +++-- .../acquisition/gnuradio_blocks/pcps_acquisition.cc | 6 ++---- .../acquisition/gnuradio_blocks/pcps_acquisition.h | 5 +++-- .../gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc | 7 +++---- .../gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h | 5 +++-- .../gnuradio_blocks/pcps_assisted_acquisition_cc.cc | 6 ++---- .../gnuradio_blocks/pcps_assisted_acquisition_cc.h | 5 +++-- .../gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc | 7 ++----- .../gnuradio_blocks/pcps_cccwsr_acquisition_cc.h | 5 +++-- .../gnuradio_blocks/pcps_opencl_acquisition_cc.cc | 9 ++------- .../gnuradio_blocks/pcps_opencl_acquisition_cc.h | 4 ++-- .../gnuradio_blocks/pcps_quicksync_acquisition_cc.cc | 7 ++----- .../gnuradio_blocks/pcps_quicksync_acquisition_cc.h | 5 ++--- .../gnuradio_blocks/pcps_tong_acquisition_cc.cc | 8 +++----- .../gnuradio_blocks/pcps_tong_acquisition_cc.h | 4 ++-- 18 files changed, 44 insertions(+), 63 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc index 3c5a7e76a..41a5ab093 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc @@ -145,10 +145,10 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit } // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = dump; @@ -210,9 +210,6 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::~galileo_e5a_noncoherentIQ_acquisi } } - delete d_fft_if; - delete d_ifft; - try { if (d_dump) diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h index adecd3a87..7b6b9925f 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -122,8 +123,8 @@ private: gr_complex* d_fft_code_Q_A; gr_complex* d_fft_code_Q_B; gr_complex* d_inbuffer; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; unsigned int d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc index 4d94413db..2fcdc7afe 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc @@ -87,10 +87,10 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc( d_magnitude = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = dump; @@ -123,9 +123,6 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc() volk_gnsssdr_free(d_fft_code_B); volk_gnsssdr_free(d_magnitude); - delete d_ifft; - delete d_fft_if; - try { if (d_dump) diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h index b080c1ef4..1dd5b5e78 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -105,8 +106,8 @@ private: uint32_t d_num_doppler_bins; gr_complex* d_fft_code_A; gr_complex* d_fft_code_B; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; uint32_t d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index c336587cb..39270efe3 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -140,10 +140,10 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu d_input_signal = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); d_gnss_synchro = nullptr; d_grid_doppler_wipeoffs = nullptr; @@ -237,8 +237,6 @@ pcps_acquisition::~pcps_acquisition() volk_gnsssdr_free(d_magnitude); volk_gnsssdr_free(d_tmp_buffer); volk_gnsssdr_free(d_input_signal); - delete d_ifft; - delete d_fft_if; volk_gnsssdr_free(d_data_buffer); if (d_cshort) { diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h index 42e2df1bc..b7b27c70c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h @@ -62,6 +62,7 @@ #include // for gr_vector_const_void_star #include // for lv_16sc_t #include +#include #include #include @@ -138,8 +139,8 @@ private: gr_complex* d_fft_codes; gr_complex* d_data_buffer; lv_16sc_t* d_data_buffer_sc; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; arma::fmat grid_; arma::fmat narrow_grid_; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index 07afa1d60..1a3642af9 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -93,10 +93,10 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_10_ms_buffer = static_cast(volk_gnsssdr_malloc(50 * d_samples_per_ms * sizeof(gr_complex), volk_gnsssdr_get_alignment())); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = conf_.dump; @@ -168,6 +168,7 @@ unsigned int pcps_acquisition_fine_doppler_cc::nextPowerOf2(unsigned int n) return n; } + void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step) { d_doppler_step = doppler_step; @@ -208,8 +209,6 @@ pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc() volk_gnsssdr_free(d_fft_codes); volk_gnsssdr_free(d_magnitude); volk_gnsssdr_free(d_10_ms_buffer); - delete d_ifft; - delete d_fft_if; free_grid_memory(); } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h index 2f992c1fc..6da3e51e8 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -109,8 +110,8 @@ private: float** d_grid_data; gr_complex** d_grid_doppler_wipeoffs; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; unsigned int d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index 37690b1b6..20221f51b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -82,10 +82,10 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc( d_carrier = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = dump; @@ -129,8 +129,6 @@ pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc() { volk_gnsssdr_free(d_carrier); volk_gnsssdr_free(d_fft_codes); - delete d_ifft; - delete d_fft_if; try { if (d_dump) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h index e05aa2266..208725b1f 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -125,8 +126,8 @@ private: float** d_grid_data; gr_complex** d_grid_doppler_wipeoffs; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; uint32_t d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc index 07a208be8..646885ee7 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc @@ -97,10 +97,10 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc( d_magnitude = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = dump; @@ -137,9 +137,6 @@ pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc() volk_gnsssdr_free(d_correlation_minus); volk_gnsssdr_free(d_magnitude); - delete d_ifft; - delete d_fft_if; - try { if (d_dump) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h index 3c53c54cc..7d5bc3b0e 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -100,8 +101,8 @@ private: uint32_t d_num_doppler_bins; gr_complex* d_fft_code_data; gr_complex* d_fft_code_pilot; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; uint32_t d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc index 1e8eef7c5..1516791c4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc @@ -131,10 +131,10 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc( if (d_opencl != 0) { // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); } // For dumping samples into a file @@ -179,11 +179,6 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc() clFFT_DestroyPlan(d_cl_fft_plan); } - else - { - delete d_ifft; - delete d_fft_if; - } try { diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h index 919881c92..bab96e123 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h @@ -123,8 +123,8 @@ private: gr_complex** d_grid_doppler_wipeoffs; uint32_t d_num_doppler_bins; gr_complex* d_fft_codes; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; uint32_t d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc index 8c88d614b..fe14f7aa6 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc @@ -106,9 +106,9 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc( d_code = new gr_complex[d_samples_per_code](); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = dump; @@ -122,7 +122,6 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc( d_threshold = 0; d_doppler_step = 0; d_grid_doppler_wipeoffs = nullptr; - d_fft_if2 = nullptr; d_gnss_synchro = nullptr; d_code_phase = 0; d_doppler_freq = 0; @@ -150,8 +149,6 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc() volk_gnsssdr_free(d_magnitude); volk_gnsssdr_free(d_magnitude_folded); - delete d_ifft; - delete d_fft_if; delete d_code; delete d_possible_delay; delete d_corr_output_f; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h index c03c0e916..17e80aa95 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h @@ -136,9 +136,8 @@ private: gr_complex** d_grid_doppler_wipeoffs; uint32_t d_num_doppler_bins; gr_complex* d_fft_codes; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_fft_if2; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; uint32_t d_code_phase; float d_doppler_freq; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc index 60b326d8d..3238161bd 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc @@ -113,10 +113,10 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc( d_magnitude = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); // Direct FFT - d_fft_if = new gr::fft::fft_complex(d_fft_size, true); + d_fft_if = std::make_shared(d_fft_size, true); // Inverse FFT - d_ifft = new gr::fft::fft_complex(d_fft_size, false); + d_ifft = std::make_shared(d_fft_size, false); // For dumping samples into a file d_dump = dump; @@ -134,6 +134,7 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc( d_channel = 0; } + pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc() { if (d_num_doppler_bins > 0) @@ -150,9 +151,6 @@ pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc() volk_gnsssdr_free(d_fft_codes); volk_gnsssdr_free(d_magnitude); - delete d_ifft; - delete d_fft_if; - try { if (d_dump) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h index a88c488b2..5c8f4c81c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h @@ -121,8 +121,8 @@ private: uint32_t d_num_doppler_bins; gr_complex* d_fft_codes; float** d_grid_data; - gr::fft::fft_complex* d_fft_if; - gr::fft::fft_complex* d_ifft; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; Gnss_Synchro* d_gnss_synchro; uint32_t d_code_phase; float d_doppler_freq; From ce1e160f1095fab15dedab31ab81a406868284f1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 14:32:40 +0200 Subject: [PATCH 23/64] Remove deletes when using Matio --- src/algorithms/PVT/libs/rtklib_solver.cc | 171 ++++++------------ .../gnuradio_blocks/hybrid_observables_gs.cc | 96 +++------- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 133 +++++--------- .../dll_pll_veml_tracking_fpga.cc | 133 +++++--------- ...glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 108 ++++------- ...glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 108 ++++------- .../glonass_l1_ca_dll_pll_tracking_cc.cc | 109 ++++------- ...glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc | 109 ++++------- ...glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc | 108 ++++------- .../glonass_l2_ca_dll_pll_tracking_cc.cc | 108 ++++------- .../gps_l1_ca_kf_tracking_cc.cc | 133 +++++--------- 11 files changed, 432 insertions(+), 884 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 79d52b467..ebb359b0f 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -64,6 +64,7 @@ #include #include #include +#include Rtklib_Solver::Rtklib_Solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, const rtk_t &rtk) @@ -172,34 +173,34 @@ bool Rtklib_Solver::save_matfile() return false; } - auto *TOW_at_current_symbol_ms = new uint32_t[num_epoch]; - auto *week = new uint32_t[num_epoch]; - auto *RX_time = new double[num_epoch]; - auto *user_clk_offset = new double[num_epoch]; - auto *pos_x = new double[num_epoch]; - auto *pos_y = new double[num_epoch]; - auto *pos_z = new double[num_epoch]; - auto *vel_x = new double[num_epoch]; - auto *vel_y = new double[num_epoch]; - auto *vel_z = new double[num_epoch]; - auto *cov_xx = new double[num_epoch]; - auto *cov_yy = new double[num_epoch]; - auto *cov_zz = new double[num_epoch]; - auto *cov_xy = new double[num_epoch]; - auto *cov_yz = new double[num_epoch]; - auto *cov_zx = new double[num_epoch]; - auto *latitude = new double[num_epoch]; - auto *longitude = new double[num_epoch]; - auto *height = new double[num_epoch]; - auto *valid_sats = new uint8_t[num_epoch]; - auto *solution_status = new uint8_t[num_epoch]; - auto *solution_type = new uint8_t[num_epoch]; - auto *AR_ratio_factor = new float[num_epoch]; - auto *AR_ratio_threshold = new float[num_epoch]; - auto *gdop = new double[num_epoch]; - auto *pdop = new double[num_epoch]; - auto *hdop = new double[num_epoch]; - auto *vdop = new double[num_epoch]; + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto week = std::vector(num_epoch); + auto RX_time = std::vector(num_epoch); + auto user_clk_offset = std::vector(num_epoch); + auto pos_x = std::vector(num_epoch); + auto pos_y = std::vector(num_epoch); + auto pos_z = std::vector(num_epoch); + auto vel_x = std::vector(num_epoch); + auto vel_y = std::vector(num_epoch); + auto vel_z = std::vector(num_epoch); + auto cov_xx = std::vector(num_epoch); + auto cov_yy = std::vector(num_epoch); + auto cov_zz = std::vector(num_epoch); + auto cov_xy = std::vector(num_epoch); + auto cov_yz = std::vector(num_epoch); + auto cov_zx = std::vector(num_epoch); + auto latitude = std::vector(num_epoch); + auto longitude = std::vector(num_epoch); + auto height = std::vector(num_epoch); + auto valid_sats = std::vector(num_epoch); + auto solution_status = std::vector(num_epoch); + auto solution_type = std::vector(num_epoch); + auto AR_ratio_factor = std::vector(num_epoch); + auto AR_ratio_threshold = std::vector(num_epoch); + auto gdop = std::vector(num_epoch); + auto pdop = std::vector(num_epoch); + auto hdop = std::vector(num_epoch); + auto vdop = std::vector(num_epoch); try { @@ -242,35 +243,6 @@ bool Rtklib_Solver::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] TOW_at_current_symbol_ms; - delete[] week; - delete[] RX_time; - delete[] user_clk_offset; - delete[] pos_x; - delete[] pos_y; - delete[] pos_z; - delete[] vel_x; - delete[] vel_y; - delete[] vel_z; - delete[] cov_xx; - delete[] cov_yy; - delete[] cov_zz; - delete[] cov_xy; - delete[] cov_yz; - delete[] cov_zx; - delete[] latitude; - delete[] longitude; - delete[] height; - delete[] valid_sats; - delete[] solution_status; - delete[] solution_type; - delete[] AR_ratio_factor; - delete[] AR_ratio_threshold; - delete[] gdop; - delete[] pdop; - delete[] hdop; - delete[] vdop; - return false; } @@ -284,149 +256,120 @@ bool Rtklib_Solver::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims, TOW_at_current_symbol_ms, 0); + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims, TOW_at_current_symbol_ms.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims, week, 0); + matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims, week.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time, 0); + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, user_clk_offset, 0); + matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, user_clk_offset.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_x, 0); + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_x.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_y, 0); + matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_y.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_z, 0); + matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_z.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_x, 0); + matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_x.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_y, 0); + matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_y.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_z, 0); + matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_z.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_xx, 0); + matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_xx.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_yy, 0); + matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_yy.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_zz, 0); + matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_zz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("cov_xy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_xy, 0); + matvar = Mat_VarCreate("cov_xy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_xy.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("cov_yz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_yz, 0); + matvar = Mat_VarCreate("cov_yz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_yz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("cov_zx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_zx, 0); + matvar = Mat_VarCreate("cov_zx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_zx.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, latitude, 0); + matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, longitude, 0); + matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, longitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, height, 0); + matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, height.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims, valid_sats, 0); + matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims, valid_sats.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("solution_status", MAT_C_UINT8, MAT_T_UINT8, 2, dims, solution_status, 0); + matvar = Mat_VarCreate("solution_status", MAT_C_UINT8, MAT_T_UINT8, 2, dims, solution_status.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("solution_type", MAT_C_UINT8, MAT_T_UINT8, 2, dims, solution_type, 0); + matvar = Mat_VarCreate("solution_type", MAT_C_UINT8, MAT_T_UINT8, 2, dims, solution_type.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("AR_ratio_factor", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, AR_ratio_factor, 0); + matvar = Mat_VarCreate("AR_ratio_factor", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, AR_ratio_factor.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("AR_ratio_threshold", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, AR_ratio_threshold, 0); + matvar = Mat_VarCreate("AR_ratio_threshold", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, AR_ratio_threshold.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, gdop, 0); + matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, gdop.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pdop, 0); + matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pdop.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, hdop, 0); + matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, hdop.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vdop, 0); + matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vdop.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] TOW_at_current_symbol_ms; - delete[] week; - delete[] RX_time; - delete[] user_clk_offset; - delete[] pos_x; - delete[] pos_y; - delete[] pos_z; - delete[] vel_x; - delete[] vel_y; - delete[] vel_z; - delete[] cov_xx; - delete[] cov_yy; - delete[] cov_zz; - delete[] cov_xy; - delete[] cov_yz; - delete[] cov_zx; - delete[] latitude; - delete[] longitude; - delete[] height; - delete[] valid_sats; - delete[] solution_status; - delete[] solution_type; - delete[] AR_ratio_factor; - delete[] AR_ratio_threshold; - delete[] gdop; - delete[] pdop; - delete[] hdop; - delete[] vdop; - return true; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index cceb030e6..91e4760f8 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -226,24 +226,14 @@ int32_t hybrid_observables_gs::save_matfile() { return 1; } - auto **RX_time = new double *[d_nchannels_out]; - auto **TOW_at_current_symbol_s = new double *[d_nchannels_out]; - auto **Carrier_Doppler_hz = new double *[d_nchannels_out]; - auto **Carrier_phase_cycles = new double *[d_nchannels_out]; - auto **Pseudorange_m = new double *[d_nchannels_out]; - auto **PRN = new double *[d_nchannels_out]; - auto **Flag_valid_pseudorange = new double *[d_nchannels_out]; - for (uint32_t i = 0; i < d_nchannels_out; i++) - { - RX_time[i] = new double[num_epoch]; - TOW_at_current_symbol_s[i] = new double[num_epoch]; - Carrier_Doppler_hz[i] = new double[num_epoch]; - Carrier_phase_cycles[i] = new double[num_epoch]; - Pseudorange_m[i] = new double[num_epoch]; - PRN[i] = new double[num_epoch]; - Flag_valid_pseudorange[i] = new double[num_epoch]; - } + auto RX_time = std::vector>(d_nchannels_out, std::vector(num_epoch)); + auto TOW_at_current_symbol_s = std::vector>(d_nchannels_out, std::vector(num_epoch)); + auto Carrier_Doppler_hz = std::vector>(d_nchannels_out, std::vector(num_epoch)); + auto Carrier_phase_cycles = std::vector>(d_nchannels_out, std::vector(num_epoch)); + auto Pseudorange_m = std::vector>(d_nchannels_out, std::vector(num_epoch)); + auto PRN = std::vector>(d_nchannels_out, std::vector(num_epoch)); + auto Flag_valid_pseudorange = std::vector>(d_nchannels_out, std::vector(num_epoch)); try { @@ -268,34 +258,17 @@ int32_t hybrid_observables_gs::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - for (uint32_t i = 0; i < d_nchannels_out; i++) - { - delete[] RX_time[i]; - delete[] TOW_at_current_symbol_s[i]; - delete[] Carrier_Doppler_hz[i]; - delete[] Carrier_phase_cycles[i]; - delete[] Pseudorange_m[i]; - delete[] PRN[i]; - delete[] Flag_valid_pseudorange[i]; - } - delete[] RX_time; - delete[] TOW_at_current_symbol_s; - delete[] Carrier_Doppler_hz; - delete[] Carrier_phase_cycles; - delete[] Pseudorange_m; - delete[] PRN; - delete[] Flag_valid_pseudorange; - return 1; } - auto *RX_time_aux = new double[d_nchannels_out * num_epoch]; - auto *TOW_at_current_symbol_s_aux = new double[d_nchannels_out * num_epoch]; - auto *Carrier_Doppler_hz_aux = new double[d_nchannels_out * num_epoch]; - auto *Carrier_phase_cycles_aux = new double[d_nchannels_out * num_epoch]; - auto *Pseudorange_m_aux = new double[d_nchannels_out * num_epoch]; - auto *PRN_aux = new double[d_nchannels_out * num_epoch]; - auto *Flag_valid_pseudorange_aux = new double[d_nchannels_out * num_epoch]; + auto RX_time_aux = std::vector(d_nchannels_out * num_epoch); + auto TOW_at_current_symbol_s_aux = std::vector(d_nchannels_out * num_epoch); + auto Carrier_Doppler_hz_aux = std::vector(d_nchannels_out * num_epoch); + auto Carrier_phase_cycles_aux = std::vector(d_nchannels_out * num_epoch); + auto Pseudorange_m_aux = std::vector(d_nchannels_out * num_epoch); + auto PRN_aux = std::vector(d_nchannels_out * num_epoch); + auto Flag_valid_pseudorange_aux = std::vector(d_nchannels_out * num_epoch); + uint32_t k = 0U; for (int64_t j = 0; j < num_epoch; j++) { @@ -325,61 +298,36 @@ int32_t hybrid_observables_gs::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {static_cast(d_nchannels_out), static_cast(num_epoch)}; - matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("TOW_at_current_symbol_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, TOW_at_current_symbol_s_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("TOW_at_current_symbol_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, TOW_at_current_symbol_s_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Carrier_Doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_Doppler_hz_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("Carrier_Doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_Doppler_hz_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Carrier_phase_cycles", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_phase_cycles_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("Carrier_phase_cycles", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_phase_cycles_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Pseudorange_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Pseudorange_m_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("Pseudorange_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Pseudorange_m_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, PRN_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("PRN", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, PRN_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Flag_valid_pseudorange", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Flag_valid_pseudorange_aux, MAT_F_DONT_COPY_DATA); + matvar = Mat_VarCreate("Flag_valid_pseudorange", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Flag_valid_pseudorange_aux.data(), MAT_F_DONT_COPY_DATA); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - for (uint32_t i = 0; i < d_nchannels_out; i++) - { - delete[] RX_time[i]; - delete[] TOW_at_current_symbol_s[i]; - delete[] Carrier_Doppler_hz[i]; - delete[] Carrier_phase_cycles[i]; - delete[] Pseudorange_m[i]; - delete[] PRN[i]; - delete[] Flag_valid_pseudorange[i]; - } - delete[] RX_time; - delete[] TOW_at_current_symbol_s; - delete[] Carrier_Doppler_hz; - delete[] Carrier_phase_cycles; - delete[] Pseudorange_m; - delete[] PRN; - delete[] Flag_valid_pseudorange; - - delete[] RX_time_aux; - delete[] TOW_at_current_symbol_s_aux; - delete[] Carrier_Doppler_hz_aux; - delete[] Carrier_phase_cycles_aux; - delete[] Pseudorange_m_aux; - delete[] PRN_aux; - delete[] Flag_valid_pseudorange_aux; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index dbe9fccca..92d50e39c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -67,6 +67,7 @@ #include // for cout, cerr #include #include +#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -1361,28 +1362,28 @@ int32_t dll_pll_veml_tracking::save_matfile() { return 1; } - auto *abs_VE = new float[num_epoch]; - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *abs_VL = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new float[num_epoch]; - auto *carrier_doppler_hz = new float[num_epoch]; - auto *carrier_doppler_rate_hz = new float[num_epoch]; - auto *code_freq_chips = new float[num_epoch]; - auto *code_freq_rate_chips = new float[num_epoch]; - auto *carr_error_hz = new float[num_epoch]; - auto *carr_error_filt_hz = new float[num_epoch]; - auto *code_error_chips = new float[num_epoch]; - auto *code_error_filt_chips = new float[num_epoch]; - auto *CN0_SNV_dB_Hz = new float[num_epoch]; - auto *carrier_lock_test = new float[num_epoch]; - auto *aux1 = new float[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_doppler_rate_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto code_freq_rate_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -1419,28 +1420,6 @@ int32_t dll_pll_veml_tracking::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -1454,117 +1433,95 @@ int32_t dll_pll_veml_tracking::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index d555aecb8..bf8645d3b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -58,6 +58,7 @@ #include #include #include +#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -1036,28 +1037,28 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() { return 1; } - auto *abs_VE = new float[num_epoch]; - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *abs_VL = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new float[num_epoch]; - auto *carrier_doppler_hz = new float[num_epoch]; - auto *carrier_doppler_rate_hz = new float[num_epoch]; - auto *code_freq_chips = new float[num_epoch]; - auto *code_freq_rate_chips = new float[num_epoch]; - auto *carr_error_hz = new float[num_epoch]; - auto *carr_error_filt_hz = new float[num_epoch]; - auto *code_error_chips = new float[num_epoch]; - auto *code_error_filt_chips = new float[num_epoch]; - auto *CN0_SNV_dB_Hz = new float[num_epoch]; - auto *carrier_lock_test = new float[num_epoch]; - auto *aux1 = new float[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_doppler_rate_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto code_freq_rate_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -1094,28 +1095,6 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -1129,117 +1108,95 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index 2c51c403f..6e8e45964 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -386,24 +386,24 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile() { return 1; } - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new double[num_epoch]; - auto *carrier_doppler_hz = new double[num_epoch]; - auto *code_freq_chips = new double[num_epoch]; - auto *carr_error_hz = new double[num_epoch]; - auto *carr_error_filt_hz = new double[num_epoch]; - auto *code_error_chips = new double[num_epoch]; - auto *code_error_filt_chips = new double[num_epoch]; - auto *CN0_SNV_dB_Hz = new double[num_epoch]; - auto *carrier_lock_test = new double[num_epoch]; - auto *aux1 = new double[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -436,24 +436,6 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -467,97 +449,79 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index a055283b3..8a4fb209d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -333,24 +333,24 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile() { return 1; } - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new double[num_epoch]; - auto *carrier_doppler_hz = new double[num_epoch]; - auto *code_freq_chips = new double[num_epoch]; - auto *carr_error_hz = new double[num_epoch]; - auto *carr_error_filt_hz = new double[num_epoch]; - auto *code_error_chips = new double[num_epoch]; - auto *code_error_filt_chips = new double[num_epoch]; - auto *CN0_SNV_dB_Hz = new double[num_epoch]; - auto *carrier_lock_test = new double[num_epoch]; - auto *aux1 = new double[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -383,24 +383,6 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -414,97 +396,79 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc index 21e2d29c9..9d9bad587 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc @@ -52,6 +52,7 @@ #include #include #include +#include #define CN0_ESTIMATION_SAMPLES 10 @@ -338,24 +339,24 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile() { return 1; } - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new double[num_epoch]; - auto *carrier_doppler_hz = new double[num_epoch]; - auto *code_freq_chips = new double[num_epoch]; - auto *carr_error_hz = new double[num_epoch]; - auto *carr_error_filt_hz = new double[num_epoch]; - auto *code_error_chips = new double[num_epoch]; - auto *code_error_filt_chips = new double[num_epoch]; - auto *CN0_SNV_dB_Hz = new double[num_epoch]; - auto *carrier_lock_test = new double[num_epoch]; - auto *aux1 = new double[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -388,24 +389,6 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -419,97 +402,79 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index cd2905d41..ab5fd3dab 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -52,6 +52,7 @@ #include #include #include +#include #define CN0_ESTIMATION_SAMPLES 10 @@ -384,24 +385,24 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile() { return 1; } - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new double[num_epoch]; - auto *carrier_doppler_hz = new double[num_epoch]; - auto *code_freq_chips = new double[num_epoch]; - auto *carr_error_hz = new double[num_epoch]; - auto *carr_error_filt_hz = new double[num_epoch]; - auto *code_error_chips = new double[num_epoch]; - auto *code_error_filt_chips = new double[num_epoch]; - auto *CN0_SNV_dB_Hz = new double[num_epoch]; - auto *carrier_lock_test = new double[num_epoch]; - auto *aux1 = new double[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -434,24 +435,6 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -465,97 +448,79 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc index d98687699..88bca3c7f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc @@ -332,24 +332,24 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile() { return 1; } - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new double[num_epoch]; - auto *carrier_doppler_hz = new double[num_epoch]; - auto *code_freq_chips = new double[num_epoch]; - auto *carr_error_hz = new double[num_epoch]; - auto *carr_error_filt_hz = new double[num_epoch]; - auto *code_error_chips = new double[num_epoch]; - auto *code_error_filt_chips = new double[num_epoch]; - auto *CN0_SNV_dB_Hz = new double[num_epoch]; - auto *carrier_lock_test = new double[num_epoch]; - auto *aux1 = new double[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -382,24 +382,6 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -413,97 +395,79 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc index ba11fa86c..4e1537925 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc @@ -338,24 +338,24 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile() { return 1; } - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new double[num_epoch]; - auto *carrier_doppler_hz = new double[num_epoch]; - auto *code_freq_chips = new double[num_epoch]; - auto *carr_error_hz = new double[num_epoch]; - auto *carr_error_filt_hz = new double[num_epoch]; - auto *code_error_chips = new double[num_epoch]; - auto *code_error_filt_chips = new double[num_epoch]; - auto *CN0_SNV_dB_Hz = new double[num_epoch]; - auto *carrier_lock_test = new double[num_epoch]; - auto *aux1 = new double[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -388,24 +388,6 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -419,97 +401,79 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index e9dbc1f3a..d76f481cd 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -54,6 +54,7 @@ #include #include #include +#include gps_l1_ca_kf_tracking_cc_sptr @@ -434,28 +435,28 @@ int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile() { return 1; } - auto *abs_VE = new float[num_epoch]; - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *abs_VL = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new float[num_epoch]; - auto *carrier_doppler_hz = new float[num_epoch]; - auto *carrier_dopplerrate_hz2 = new float[num_epoch]; - auto *code_freq_chips = new float[num_epoch]; - auto *carr_error_hz = new float[num_epoch]; - auto *carr_noise_sigma2 = new float[num_epoch]; - auto *carr_error_filt_hz = new float[num_epoch]; - auto *code_error_chips = new float[num_epoch]; - auto *code_error_filt_chips = new float[num_epoch]; - auto *CN0_SNV_dB_Hz = new float[num_epoch]; - auto *carrier_lock_test = new float[num_epoch]; - auto *aux1 = new float[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_dopplerrate_hz2 = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_noise_sigma2 = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { @@ -492,28 +493,6 @@ int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_dopplerrate_hz2; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_noise_sigma2; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -527,117 +506,95 @@ int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2, 0); + matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2, 0); + matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_dopplerrate_hz2; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_noise_sigma2; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } From 05c41d41e142e5c2791daa114d21b4b7353a909a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 15:15:27 +0200 Subject: [PATCH 24/64] Remove deletes --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 7 +++---- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.h | 3 ++- .../gnuradio_blocks/dll_pll_veml_tracking_fpga.cc | 8 +++----- .../tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h | 3 ++- .../galileo_e1_tcp_connector_tracking_cc.cc | 7 +++---- .../galileo_e1_tcp_connector_tracking_cc.h | 3 ++- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 7 +++---- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.h | 3 ++- .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 7 +++---- .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.h | 3 ++- .../gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc | 7 +++---- .../gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h | 3 ++- .../glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc | 7 +++---- .../glonass_l2_ca_dll_pll_c_aid_tracking_cc.h | 3 ++- .../glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc | 7 +++---- .../glonass_l2_ca_dll_pll_c_aid_tracking_sc.h | 3 ++- .../gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc | 7 +++---- .../gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.h | 3 ++- .../gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 7 +++---- .../gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h | 3 ++- .../tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc | 7 +++---- .../tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h | 3 ++- .../gps_l1_ca_tcp_connector_tracking_cc.cc | 7 +++---- .../gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h | 3 ++- 24 files changed, 60 insertions(+), 61 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 92d50e39c..e26712291 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -449,7 +449,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[trk_parameters.cn0_samples]; + d_Prompt_buffer = std::vector(trk_parameters.cn0_samples); d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; d_carrier_lock_fail_counter = 0; @@ -824,7 +824,6 @@ dll_pll_veml_tracking::~dll_pll_veml_tracking() volk_gnsssdr_free(d_data_code); correlator_data_cpu.free(); } - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -887,10 +886,10 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ d_Prompt_buffer[d_cn0_estimation_counter % trk_parameters.cn0_samples] = d_P_accu; d_cn0_estimation_counter++; // Code lock indicator - float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); + float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); // Carrier lock indicator - d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer, 1)); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); //d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); // Loss of lock detection if (!d_pull_in_transitory) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 38969900a..170a3ddb5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -46,6 +46,7 @@ #include // for int32_t #include // for string, ofstream #include // for pair +#include class Gnss_Synchro; class dll_pll_veml_tracking; @@ -201,7 +202,7 @@ private: double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; boost::circular_buffer d_Prompt_circular_buffer; - gr_complex *d_Prompt_buffer; + std::vector d_Prompt_buffer; Exponential_Smoother d_cn0_smoother; Exponential_Smoother d_carrier_lock_test_smoother; // file dump diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index bf8645d3b..456c5814b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -361,7 +361,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[trk_parameters.cn0_samples]; + d_Prompt_buffer = std::vector(trk_parameters.cn0_samples); d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; d_cn0_smoother = Exponential_Smoother(); @@ -537,7 +537,6 @@ dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_Prompt_Data); - delete[] d_Prompt_buffer; multicorrelator_fpga->free(); } catch (const std::exception &ex) @@ -601,10 +600,10 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra { d_cn0_estimation_counter = 0; // Code lock indicator - float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); + float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), trk_parameters.cn0_samples); // Loss of lock detection if (!d_pull_in_transitory) { @@ -734,7 +733,6 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() d_code_error_filt_chips = 0.0; d_current_symbol = 0; d_Prompt_circular_buffer.clear(); - //d_Prompt_buffer_deque.clear(); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; d_carr_ph_history.clear(); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 0cb708323..bb63e0f89 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -48,6 +48,7 @@ #include // for shared_ptr #include #include // for pair +#include class Fpga_Multicorrelator_8sc; class Gnss_Synchro; @@ -201,7 +202,7 @@ private: double d_carrier_lock_threshold; boost::circular_buffer d_Prompt_circular_buffer; //std::deque d_Prompt_buffer_deque; - gr_complex *d_Prompt_buffer; + std::vector d_Prompt_buffer; Exponential_Smoother d_cn0_smoother; // file dump diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index 7479c0923..0966fb809 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -163,7 +163,7 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -247,7 +247,6 @@ Galileo_E1_Tcp_Connector_Tracking_cc::~Galileo_E1_Tcp_Connector_Tracking_cc() volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; d_tcp_com.close_tcp_connection(d_port); multicorrelator_cpu.free(); } @@ -415,10 +414,10 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GALILEO_E1_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GALILEO_E1_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h index 423eef516..dfd71cd9d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h @@ -47,6 +47,7 @@ #include #include #include +#include class Galileo_E1_Tcp_Connector_Tracking_cc; @@ -166,7 +167,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex *d_Prompt_buffer; + std::vector d_Prompt_buffer; float d_carrier_lock_test; float d_CN0_SNV_dB_Hz; float d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index 6e8e45964..79bf4038d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -175,7 +175,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -345,7 +345,6 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::~glonass_l1_ca_dll_pll_c_aid_tracking_c volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -762,9 +761,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h index 93c43c35a..394966f28 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h @@ -50,6 +50,7 @@ #include #include #include +#include class glonass_l1_ca_dll_pll_c_aid_tracking_cc; @@ -180,7 +181,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index 8a4fb209d..5b5e8fdc8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -173,7 +173,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -513,7 +513,6 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::~glonass_l1_ca_dll_pll_c_aid_tracking_s volk_gnsssdr_free(d_ca_code_16sc); volk_gnsssdr_free(d_correlator_outs_16sc); - delete[] d_Prompt_buffer; multicorrelator_cpu_16sc.free(); } @@ -752,9 +751,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h index a58fee4aa..a220d3690 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h @@ -51,6 +51,7 @@ #include #include #include +#include class glonass_l1_ca_dll_pll_c_aid_tracking_sc; @@ -183,7 +184,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc index 9d9bad587..ec8a6fd92 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc @@ -146,7 +146,7 @@ Glonass_L1_Ca_Dll_Pll_Tracking_cc::Glonass_L1_Ca_Dll_Pll_Tracking_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -298,7 +298,6 @@ Glonass_L1_Ca_Dll_Pll_Tracking_cc::~Glonass_L1_Ca_Dll_Pll_Tracking_cc() volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -621,9 +620,9 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h index 158eaa761..d75e308c5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h @@ -47,6 +47,7 @@ #include #include #include +#include class Glonass_L1_Ca_Dll_Pll_Tracking_cc; @@ -148,7 +149,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index ab5fd3dab..00d3242a7 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -173,7 +173,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -344,7 +344,6 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::~glonass_l2_ca_dll_pll_c_aid_tracking_c volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -761,9 +760,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h index 44b711243..90d47b04c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h @@ -48,6 +48,7 @@ #include #include #include +#include class glonass_l2_ca_dll_pll_c_aid_tracking_cc; @@ -178,7 +179,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc index 88bca3c7f..a8ae96797 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc @@ -172,7 +172,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::glonass_l2_ca_dll_pll_c_aid_tracking_sc // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -512,7 +512,6 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::~glonass_l2_ca_dll_pll_c_aid_tracking_s volk_gnsssdr_free(d_ca_code_16sc); volk_gnsssdr_free(d_correlator_outs_16sc); - delete[] d_Prompt_buffer; multicorrelator_cpu_16sc.free(); } @@ -751,9 +750,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h index d451da17b..74309f51a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h @@ -49,6 +49,7 @@ #include #include #include +#include class glonass_l2_ca_dll_pll_c_aid_tracking_sc; @@ -181,7 +182,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc index 4e1537925..125d93255 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc @@ -145,7 +145,7 @@ Glonass_L2_Ca_Dll_Pll_Tracking_cc::Glonass_L2_Ca_Dll_Pll_Tracking_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -297,7 +297,6 @@ Glonass_L2_Ca_Dll_Pll_Tracking_cc::~Glonass_L2_Ca_Dll_Pll_Tracking_cc() volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -620,9 +619,9 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.h index d72137d29..06773090b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.h @@ -45,6 +45,7 @@ #include #include #include +#include class Glonass_L2_Ca_Dll_Pll_Tracking_cc; @@ -146,7 +147,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index de2d017bc..b74eee6b5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -138,7 +138,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -270,7 +270,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc() cudaFreeHost(d_correlator_outs); cudaFreeHost(d_local_code_shift_chips); cudaFreeHost(d_ca_code); - delete[] d_Prompt_buffer; multicorrelator_gpu->free_cuda(); delete (multicorrelator_gpu); } @@ -437,9 +436,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h index 987af5686..5282caacb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h @@ -44,6 +44,7 @@ #include #include #include +#include class Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc; @@ -157,7 +158,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex *d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index d76f481cd..dfe63deb4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -163,7 +163,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -394,7 +394,6 @@ Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc() volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -793,9 +792,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples); // Loss of lock detection if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h index e2faa339c..565d862a5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -50,6 +50,7 @@ #include #include #include +#include class Gps_L1_Ca_Kf_Tracking_cc; @@ -198,7 +199,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; + std::vector d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index 1cbb90d30..e1481ebdb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -149,7 +149,7 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; + d_Prompt_buffer = std::vector(FLAGS_cn0_samples); d_carrier_lock_test = 1; d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; @@ -276,7 +276,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::~Gps_L1_Ca_Tcp_Connector_Tracking_cc() volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_ca_code); d_tcp_com.close_tcp_connection(d_port); - delete[] d_Prompt_buffer; multicorrelator_cpu.free(); } catch (const std::exception &ex) @@ -450,8 +449,8 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib else { d_cn0_estimation_counter = 0; - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples); // ###### TRACKING UNLOCK NOTIFICATION ##### if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h index 3fa928a4d..106f38ab4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h @@ -44,6 +44,7 @@ #include #include #include +#include class Gps_L1_Ca_Tcp_Connector_Tracking_cc; @@ -152,7 +153,7 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; - gr_complex *d_Prompt_buffer; + std::vector d_Prompt_buffer; float d_carrier_lock_test; float d_CN0_SNV_dB_Hz; float d_carrier_lock_threshold; From 9b5a69e9ce0fa8f729247072aa4b1b326a573891 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 30 Jun 2019 19:20:34 +0200 Subject: [PATCH 25/64] Document addition of the Guidelines Support Library in the changelog --- docs/changelog | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/changelog b/docs/changelog index 3b8c0438e..97af773b1 100644 --- a/docs/changelog +++ b/docs/changelog @@ -56,6 +56,7 @@ ### Improvements in Reliability +- Included the Guidelines Support Library. General improvement of memory management, replacement of raw pointers by containers or smart pointers. - Applied clang-tidy checks and fixes related to High Integrity C++: performance-move-const-arg, modernize-use-auto, modernize-use-equals-default, modernize-use-equals-delete, modernize-use-noexcept, modernize-use-nullptr, cert-dcl21-cpp, misc-new-delete-overloads, cert-dcl58-cpp, cert-err52-cpp, cert-err60-cpp. From e17472d986a2e0b3d80c61d5cc84025e920ceac9 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 1 Jul 2019 11:00:38 +0200 Subject: [PATCH 26/64] Partial merge commit, to be fixed in next commit --- .../beidou_b1i_telemetry_decoder_gs.cc | 135 ++-- .../beidou_b1i_telemetry_decoder_gs.h | 10 +- .../galileo_telemetry_decoder_gs.cc | 109 ++-- .../galileo_telemetry_decoder_gs.h | 2 - .../gps_l1_ca_telemetry_decoder_gs.cc | 133 ++-- .../gps_l5_telemetry_decoder_gs.cc | 75 +-- .../gps_l5_telemetry_decoder_gs.h | 4 - .../gnuradio_blocks/dll_pll_veml_tracking.cc | 590 ++++++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 9 +- src/core/system_parameters/Beidou_B1I.h | 8 +- src/core/system_parameters/Beidou_B3I.h | 2 + src/core/system_parameters/GPS_L1_CA.h | 2 + 12 files changed, 413 insertions(+), 666 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 9417cd510..0ca5f5e74 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -72,56 +72,27 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite; - d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - - // Setting samples of secondary code - for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++) - { - if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_symbols[i] = 1; - } - else - { - d_secondary_code_symbols[i] = -1; - } - } + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - int32_t m = 0; if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); // Generic settings d_sample_counter = 0; @@ -141,7 +112,6 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -189,7 +159,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in void beidou_b1i_telemetry_decoder_gs::decode_word( int32_t word_counter, - const double *enc_word_symbols, + const float *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -229,7 +199,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_word( } -void beidou_b1i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) +void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -312,48 +282,36 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell // Update satellite information for DNAV decoder sat_prn = d_satellite.get_PRN(); d_nav.i_satellite_PRN = sat_prn; - d_nav.i_signal_type = 1; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) + d_nav.i_signal_type = 1; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) // Update tel dec parameters for D2 NAV Messages if (sat_prn > 0 and sat_prn < 6) { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); - d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = nullptr; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); } } @@ -403,7 +361,8 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_flag_preamble = false; - if (d_symbol_history.size() > d_required_symbols) + //std::cout << "size: " << d_symbol_history.size() << " in " << current_symbol.Prompt_I << std::endl; + if (d_symbol_history.size() >= d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -416,9 +375,15 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { corr_value += d_preamble_samples[i]; } + //std::cout << "corr: " << corr_value << ","; } + //std::cout << " final corr: " << corr_value << std::endl; } + if (abs(corr_value) >= d_samples_per_preamble) + { + std::cout << " preamble corr: " << corr_value << std::endl; + } //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -461,53 +426,38 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ // ******* SAMPLES TO SYMBOLS ******* if (corr_value > 0) //normal PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = d_symbol_history.at(i); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = d_symbol_history.at(i); } } } else // 180 deg. inverted carrier phase PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = -d_symbol_history.at(i); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = -d_symbol_history.at(i); } } } + // call the decoder decode_subframe(d_subframe_symbols); @@ -536,7 +486,6 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ } } } - // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 3a7b8debf..9180a40b8 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -39,7 +39,7 @@ #include #include // for boost::shared_ptr #include // for block -#include // for gr_vector_const_void_star +#include // for gr_vector_const_void_star #include #include #include @@ -77,19 +77,17 @@ private: beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); beidou_b1i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(double *symbols); - void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_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_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; - int32_t *d_secondary_code_symbols; - uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - double *d_subframe_symbols; + float *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index a94aebcf8..4bfcc06d3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -85,15 +85,13 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( case 1: // INAV { d_PRN_code_period_ms = static_cast(GALILEO_E1_CODE_PERIOD_MS); - d_samples_per_symbol = GALILEO_E1_B_SAMPLES_PER_SYMBOL; d_bits_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS; // set the preamble - d_samples_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol; + d_samples_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS; d_preamble_period_symbols = GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS; d_required_symbols = static_cast(GALILEO_INAV_PAGE_SYMBOLS) + d_samples_per_preamble; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_secondary_code_samples = nullptr; d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; @@ -103,31 +101,18 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( } case 2: // FNAV { - d_PRN_code_period_ms = static_cast(GALILEO_E5A_CODE_PERIOD_MS); - d_samples_per_symbol = GALILEO_FNAV_CODES_PER_SYMBOL; + d_PRN_code_period_ms = static_cast(GALILEO_E5A_CODE_PERIOD_MS * GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_bits_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; // set the preamble - d_samples_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol; - d_preamble_period_symbols = GALILEO_FNAV_CODES_PER_PAGE; - d_required_symbols = static_cast(GALILEO_FNAV_SYMBOLS_PER_PAGE) * d_samples_per_symbol + d_samples_per_preamble; + d_samples_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; + d_preamble_period_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE; + d_required_symbols = static_cast(GALILEO_FNAV_SYMBOLS_PER_PAGE) + d_samples_per_preamble; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_secondary_code_samples = static_cast(volk_gnsssdr_malloc(GALILEO_E5A_I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; - for (int32_t i = 0; i < GALILEO_E5A_I_SECONDARY_CODE_LENGTH; i++) - { - if (GALILEO_E5A_I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_samples[i] = 1; - } - else - { - d_secondary_code_samples[i] = -1; - } - } - d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_FNAV_SYMBOLS_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm break; } default: @@ -135,8 +120,6 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_samples_per_preamble = 0; d_preamble_period_symbols = 0; d_preamble_samples = nullptr; - d_secondary_code_samples = nullptr; - d_samples_per_symbol = 0U; d_PRN_code_period_ms = 0U; d_required_symbols = 0U; d_frame_length_symbols = 0U; @@ -147,7 +130,6 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( } d_page_part_symbols = static_cast(volk_gnsssdr_malloc(d_frame_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); - int32_t n = 0; for (int32_t i = 0; i < d_bits_per_preamble; i++) { switch (d_frame_type) @@ -156,45 +138,23 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( { if (GALILEO_INAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } + d_preamble_samples[i] = -1; } break; } case 2: // FNAV for E5a-I { - // Galileo E5a data channel (E5a-I) still has a secondary code - int m = 0; if (GALILEO_FNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_samples[m]; - n++; - m++; - m = m % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_samples[m]; - n++; - m++; - m = m % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = -1; } break; } @@ -235,10 +195,6 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( galileo_telemetry_decoder_gs::~galileo_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); - if (d_frame_type == 2) - { - volk_gnsssdr_free(d_secondary_code_samples); - } volk_gnsssdr_free(d_page_part_symbols); volk_gnsssdr_free(out0); volk_gnsssdr_free(out1); @@ -497,7 +453,24 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // 1. Copy the current tracking output current_symbol = in[0][0]; // add new symbol to the symbol queue - d_symbol_history.push_back(current_symbol.Prompt_I); + switch (d_frame_type) + { + case 1: // INAV + { + d_symbol_history.push_back(current_symbol.Prompt_I); + break; + } + case 2: //FNAV + { + d_symbol_history.push_back(current_symbol.Prompt_Q); + break; + } + default: + { + d_symbol_history.push_back(current_symbol.Prompt_I); + break; + } + } d_sample_counter++; // count for the processed symbols consume_each(1); d_flag_preamble = false; @@ -626,29 +599,21 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // 0. fetch the symbols into an array if (flag_PLL_180_deg_phase_locked == false) // normal PLL lock { - int k = 0; for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] = 0; - for (uint32_t m = 0; m < d_samples_per_symbol; m++) + for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] += static_cast(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); // because last symbol of the preamble is just received now! - k++; - k = k % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + d_page_part_symbols[i] = d_symbol_history.at(i + d_samples_per_preamble); // because last symbol of the preamble is just received now! } } } else // 180 deg. inverted carrier phase PLL lock { - int k = 0; for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] = 0; - for (uint32_t m = 0; m < d_samples_per_symbol; m++) // integrate samples into symbols + for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] -= static_cast(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); // because last symbol of the preamble is just received now! - k++; - k = k % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + d_page_part_symbols[i] = -d_symbol_history.at(i + d_samples_per_preamble); // because last symbol of the preamble is just received now! } } } @@ -732,7 +697,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( if (d_fnav_nav.flag_TOW_1 == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_1 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_1 = false; } @@ -740,26 +705,26 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_2 * 1000.0); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_2 = false; } else if (d_fnav_nav.flag_TOW_3 == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_3 * 1000.0); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_3 = false; } else if (d_fnav_nav.flag_TOW_4 == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_4 * 1000.0); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_4 = false; } else { - d_TOW_at_current_symbol_ms += static_cast(GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms += static_cast(GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); } break; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index 33e936d04..9d3ed657d 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -85,8 +85,6 @@ private: int32_t d_samples_per_preamble; int32_t d_preamble_period_symbols; int32_t *d_preamble_samples; - int32_t *d_secondary_code_samples; - uint32_t d_samples_per_symbol; uint32_t d_PRN_code_period_ms; uint32_t d_required_symbols; uint32_t d_frame_length_symbols; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index eb5d43f4f..612045e31 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -76,10 +76,11 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( DLOG(INFO) << "Initializing GPS L1 TELEMETRY DECODER"; d_bits_per_preamble = GPS_CA_PREAMBLE_LENGTH_BITS; - d_samples_per_preamble = d_bits_per_preamble * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; - d_preamble_period_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + //d_samples_per_preamble = d_bits_per_preamble * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_samples_per_preamble = d_bits_per_preamble; + d_preamble_period_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; // set the preamble - d_required_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_required_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; @@ -89,19 +90,19 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( { if (GPS_CA_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - d_preamble_samples[n] = 1; - n++; - } + // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) + // { + d_preamble_samples[n] = 1; + n++; + // } } else { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - d_preamble_samples[n] = -1; - n++; - } + // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) + // { + d_preamble_samples[n] = -1; + n++; + // } } } d_sample_counter = 0ULL; @@ -209,74 +210,58 @@ void gps_l1_ca_telemetry_decoder_gs::set_channel(int32_t channel) bool gps_l1_ca_telemetry_decoder_gs::decode_subframe() { char subframe[GPS_SUBFRAME_LENGTH]; - - int32_t symbol_accumulator_counter = 0; int32_t frame_bit_index = 0; int32_t word_index = 0; uint32_t GPS_frame_4bytes = 0; - float symbol_accumulator = 0; bool subframe_synchro_confirmation = true; for (float subframe_symbol : d_symbol_history) { // ******* SYMBOL TO BIT ******* - // extended correlation to bit period is enabled in tracking! - symbol_accumulator += subframe_symbol; // accumulate the input value in d_symbol_accumulator - symbol_accumulator_counter++; - if (symbol_accumulator_counter == 20) + // symbol to bit + if (subframe_symbol > 0) { - // symbol to bit - if (symbol_accumulator > 0) - { - GPS_frame_4bytes += 1; // insert the telemetry bit in LSB - //std::cout << "1"; - } - else - { - //std::cout << "0"; - } - symbol_accumulator = 0; - symbol_accumulator_counter = 0; + GPS_frame_4bytes += 1; // insert the telemetry bit in LSB + } - // ******* bits to words ****** - frame_bit_index++; - if (frame_bit_index == 30) + // ******* bits to words ****** + frame_bit_index++; + if (frame_bit_index == 30) + { + frame_bit_index = 0; + // parity check + // Each word in wordbuff is composed of: + // Bits 0 to 29 = the GPS data word + // Bits 30 to 31 = 2 LSBs of the GPS word ahead. + // prepare the extended frame [-2 -1 0 ... 30] + if (d_prev_GPS_frame_4bytes & 0x00000001) { - frame_bit_index = 0; - // parity check - // Each word in wordbuff is composed of: - // Bits 0 to 29 = the GPS data word - // Bits 30 to 31 = 2 LSBs of the GPS word ahead. - // prepare the extended frame [-2 -1 0 ... 30] - if (d_prev_GPS_frame_4bytes & 0x00000001) - { - GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000; - } - if (d_prev_GPS_frame_4bytes & 0x00000002) - { - GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000; - } - // Check that the 2 most recently logged words pass parity. Have to first - // invert the data bits according to bit 30 of the previous word. - if (GPS_frame_4bytes & 0x40000000) - { - GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) - } - // check parity. If ANY word inside the subframe fails the parity, set subframe_synchro_confirmation = false - if (not gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(GPS_frame_4bytes)) - { - subframe_synchro_confirmation = false; - } - // add word to subframe - // insert the word in the correct position of the subframe - std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t)); - word_index++; - d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame - GPS_frame_4bytes = 0; + GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000; } - else + if (d_prev_GPS_frame_4bytes & 0x00000002) { - GPS_frame_4bytes <<= 1; // shift 1 bit left the telemetry word + GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000; } + // Check that the 2 most recently logged words pass parity. Have to first + // invert the data bits according to bit 30 of the previous word. + if (GPS_frame_4bytes & 0x40000000) + { + GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) + } + // check parity. If ANY word inside the subframe fails the parity, set subframe_synchro_confirmation = false + if (not gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(GPS_frame_4bytes)) + { + subframe_synchro_confirmation = false; + } + // add word to subframe + // insert the word in the correct position of the subframe + std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t)); + word_index++; + d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame + GPS_frame_4bytes = 0; + } + else + { + GPS_frame_4bytes <<= 1; // shift 1 bit left the telemetry word } } @@ -378,10 +363,10 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { // correlate with preamble int32_t corr_value = 0; - if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) + if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_BITS) { // ******* preamble correlation ******** - for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) + for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++) { if (d_symbol_history[i] < 0.0) // symbols clipping { @@ -397,6 +382,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { d_preamble_index = d_sample_counter; // record the preamble sample stamp DLOG(INFO) << "Preamble detection for GPS L1 satellite " << this->d_satellite; + decode_subframe(); d_stat = 1; // enter into frame pre-detection status } flag_TOW_set = false; @@ -407,10 +393,10 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ // correlate with preamble int32_t corr_value = 0; int32_t preamble_diff = 0; - if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) + if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_BITS) { // ******* preamble correlation ******** - for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) + for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++) { if (d_symbol_history[i] < 0.0) // symbols clipping { @@ -438,6 +424,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { flag_PLL_180_deg_phase_locked = false; } + decode_subframe(); d_stat = 2; } else @@ -510,7 +497,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { if (flag_TOW_set == true) { - d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms += GPS_L1_CA_BIT_PERIOD_MS; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index ea8a87e44..891f12c27 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -64,7 +64,7 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry_to_trk")); d_last_valid_preamble = 0; d_sent_tlm_failed_msg = false; - d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 10; //rise alarm if 20 consecutive subframes have no valid CRC + d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SYMBOLS_PER_BIT * 10; //rise alarm if 20 consecutive subframes have no valid CRC // initialize internal vars d_dump = dump; @@ -74,23 +74,8 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( d_flag_valid_word = false; d_TOW_at_current_symbol_ms = 0U; d_TOW_at_Preamble_ms = 0U; - sym_hist.set_capacity(GPS_L5I_NH_CODE_LENGTH); - // initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); - for (int32_t aux = 0; aux < GPS_L5I_NH_CODE_LENGTH; aux++) - { - if (GPS_L5I_NH_CODE[aux] == 0) - { - bits_NH[aux] = -1.0; - } - else - { - bits_NH[aux] = 1.0; - } - } - sync_NH = false; - new_sym = false; d_sample_counter = 0; } @@ -171,9 +156,6 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u // 1. Copy the current tracking output current_synchro_data = in[0]; consume_each(1); // one by one - sym_hist.push_back(in[0].Prompt_I); - int32_t corr_NH = 0; - int32_t symbol_value = 0; // check if there is a problem with the telemetry of the current satellite d_sample_counter++; // count for the processed symbols @@ -187,61 +169,18 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u } } - // Search correlation with Neuman-Hofman Code (see IS-GPS-705D) - if (sym_hist.size() == GPS_L5I_NH_CODE_LENGTH) - { - for (int32_t i = 0; i < GPS_L5I_NH_CODE_LENGTH; i++) - { - if ((bits_NH[i] * sym_hist[i]) > 0.0) - { - corr_NH += 1; - } - else - { - corr_NH -= 1; - } - } - if (abs(corr_NH) == GPS_L5I_NH_CODE_LENGTH) - { - sync_NH = true; - if (corr_NH > 0) - { - symbol_value = 1; - } - else - { - symbol_value = -1; - } - new_sym = true; - //sym_hist.clear(); - } - else - { - sync_NH = false; - new_sym = false; - } - } - - bool flag_new_cnav_frame = false; cnav_msg_t msg; - uint32_t delay = 0; - - // add the symbol to the decoder - if (new_sym) - { - uint8_t symbol_clip = static_cast(symbol_value > 0) * 255; - flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); - new_sym = false; - } + uint32_t delay; + uint8_t symbol_clip = static_cast(current_synchro_data.Prompt_Q > 0) * 255; // 2. Add the telemetry decoder information // check if new CNAV frame is available - if (flag_new_cnav_frame == true) + if (cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay) == true) { std::bitset raw_bits; // Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder for (uint32_t i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS; i++) { - raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1U); + raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1u); } d_CNAV_Message.decode_page(raw_bits); @@ -279,7 +218,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u //check TOW update consistency uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5I_SYMBOL_PERIOD_MS; - if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > 1) + if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > GPS_L5I_SYMBOL_PERIOD_MS) { DLOG(INFO) << "Warning: GPS L5 TOW update in ch " << d_channel << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms " @@ -298,7 +237,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u { if (d_flag_valid_word) { - d_TOW_at_current_symbol_ms += GPS_L5I_PERIOD_MS; + d_TOW_at_current_symbol_ms += GPS_L5I_SYMBOL_PERIOD_MS; if (current_synchro_data.Flag_valid_symbol_output == false) { d_flag_valid_word = false; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h index 1971f3c09..9c49694d3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h @@ -91,10 +91,6 @@ private: uint32_t d_max_symbols_without_valid_frame; Gps_CNAV_Navigation_Message d_CNAV_Message; - float bits_NH[GPS_L5I_NH_CODE_LENGTH]{}; - boost::circular_buffer sym_hist; - bool sync_NH; - bool new_sym; }; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index e26712291..c90301a33 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -63,11 +63,9 @@ #include // for fill_n #include // for fmod, round, floor #include // for exception -#include -#include // for cout, cerr +#include // for cout, cerr #include #include -#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -109,6 +107,8 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_chip_rate = 0.0; d_secondary_code_length = 0U; d_secondary_code_string = nullptr; + d_data_secondary_code_length = 0U; + d_data_secondary_code_string = nullptr; d_preambles_symbols = nullptr; d_preamble_length_symbols = 0; signal_type = std::string(trk_parameters.signal); @@ -134,39 +134,17 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_signal_carrier_freq = GPS_L1_FREQ_HZ; d_code_period = GPS_L1_CA_CODE_PERIOD; d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; - d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - - // set the preamble - uint16_t preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE; - - // preamble bits to sampled symbols - d_preamble_length_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - for (uint16_t preambles_bit : preambles_bits) - { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer + // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit + // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization + d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&GPS_CA_PREAMBLE_SYMBOLS_STR); + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; } else if (signal_type == "2S") { @@ -174,19 +152,20 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + //GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; d_code_samples_per_chip = 1; // GPS L2 does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; } else if (signal_type == "L5") { d_signal_carrier_freq = GPS_L5_FREQ_HZ; d_code_period = GPS_L5I_PERIOD; d_code_chip_rate = GPS_L5I_CODE_RATE_HZ; + // symbol integration: 10 trk symbols (10 ms) = 1 tlm bit d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; @@ -194,17 +173,22 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = true; if (trk_parameters.track_pilot) { + //synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); + //remove data secondary code + //remove Neuman-Hofman Code (see IS-GPS-705D) + d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; } else { + //synchronize and remove data secondary code + //remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; } } else @@ -213,7 +197,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0U; @@ -230,6 +213,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GALILEO_E1_CODE_PERIOD; d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ; d_code_length_chips = static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS); + //Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required d_symbols_per_bit = 1; d_correlation_length_ms = 4; d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip @@ -246,7 +230,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = false; signal_pretty_name = signal_pretty_name + "B"; } - interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. } else if (signal_type == "5X") { @@ -257,20 +241,22 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GALILEO_E5A_CODE_LENGTH_CHIPS); - + d_secondary = true; if (trk_parameters.track_pilot) { - d_secondary = true; + //synchronize pilot secondary code d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; + //remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); } else { - //Do not acquire secondary code in data component. It is done in telemetry decoder - d_secondary = false; + //synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; } } else @@ -279,7 +265,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0U; @@ -302,9 +287,11 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; - interchange_iq = false; + //synchronize and remove data secondary code d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); } else if (signal_type == "B3") { @@ -318,9 +305,10 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; - interchange_iq = false; d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); } else { @@ -328,7 +316,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0; @@ -343,7 +330,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cerr << "Invalid System argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0U; @@ -589,52 +575,48 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_phase_rate_step_rad = 0.0; d_carr_ph_history.clear(); d_code_ph_history.clear(); - std::array Signal_; - std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); if (systemName == "GPS" and signal_type == "1C") { - gps_l1_ca_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); } else if (systemName == "GPS" and signal_type == "2S") { - gps_l2c_m_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); } else if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) { - gps_l5q_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); - gps_l5i_code_gen_float(gsl::span(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - gps_l5i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "1B") { if (trk_parameters.track_pilot) { - std::array pilot_signal = {{'1', 'C', '\0'}}; - - galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN); - galileo_e1_code_gen_sinboc11_float(gsl::span(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "5X") { auto *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); - std::array signal_type_ = {{'5', 'X', '\0'}}; - galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_); + galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); @@ -657,7 +639,7 @@ void dll_pll_veml_tracking::start_tracking() } else if (systemName == "Beidou" and signal_type == "B1") { - beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -666,38 +648,17 @@ void dll_pll_veml_tracking::start_tracking() d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - d_secondary_code_length = 0; - d_secondary_code_string = const_cast(&BEIDOU_B1I_D2_SECONDARY_CODE_STR); - // preamble bits to sampled symbols - d_preamble_length_symbols = 22; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(22 * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - uint32_t preambles_bits[BEIDOU_B1I_PREAMBLE_LENGTH_BITS] = {1, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0}; - for (uint32_t preambles_bit : preambles_bits) - { - for (int32_t j = 0; j < d_symbols_per_bit; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.resize(22); // Change fixed buffer size - d_symbol_history.clear(); + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR); + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } else if (systemName == "Beidou" and signal_type == "B3") { - beidou_b3i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -706,32 +667,11 @@ void dll_pll_veml_tracking::start_tracking() d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - d_secondary_code_length = 0; - d_secondary_code_string = const_cast(&BEIDOU_B3I_D2_SECONDARY_CODE_STR); - // preamble bits to sampled symbols - d_preamble_length_symbols = 22; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(22 * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - uint32_t preambles_bits[BEIDOU_B3I_PREAMBLE_LENGTH_BITS] = {1, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0}; - for (uint32_t preambles_bit : preambles_bits) - { - for (int32_t j = 0; j < d_symbols_per_bit; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.resize(22); // Change fixed buffer size - d_symbol_history.clear(); + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR); + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -1053,6 +993,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() if (trk_parameters.track_pilot) { d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); } d_P_accu_old = gr_complex(0.0, 0.0); d_carr_phase_error_hz = 0.0; @@ -1061,6 +1002,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; + d_current_data_symbol = 0; d_Prompt_circular_buffer.clear(); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; @@ -1184,12 +1126,69 @@ void dll_pll_veml_tracking::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; - d_current_symbol++; - d_current_symbol %= d_symbols_per_bit; } - // If tracking pilot, disable Costas loop + + //data secondary code roll-up + if (d_symbols_per_bit > 1) + { + if (d_data_secondary_code_length > 0) + { + if (trk_parameters.track_pilot) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + d_current_data_symbol++; + // data secondary code roll-up + d_current_data_symbol %= d_data_secondary_code_length; + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu += *d_Prompt; + } + d_current_data_symbol++; + d_current_data_symbol %= d_symbols_per_bit; + } + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu = *d_Prompt_Data; + } + else + { + d_P_data_accu = *d_Prompt; + } + } + if (trk_parameters.track_pilot) { + // If tracking pilot, disable Costas loop d_cloop = false; } else @@ -1198,8 +1197,7 @@ void dll_pll_veml_tracking::save_correlation_results() } } - -void dll_pll_veml_tracking::log_data(bool integrating) +void dll_pll_veml_tracking::log_data() { if (d_dump) { @@ -1212,29 +1210,13 @@ void dll_pll_veml_tracking::log_data(bool integrating) uint64_t tmp_long_int; if (trk_parameters.track_pilot) { - if (interchange_iq) - { - prompt_I = d_Prompt_Data->imag(); - prompt_Q = d_Prompt_Data->real(); - } - else - { - prompt_I = d_Prompt_Data->real(); - prompt_Q = d_Prompt_Data->imag(); - } + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); } else { - if (interchange_iq) - { - prompt_I = d_Prompt->imag(); - prompt_Q = d_Prompt->real(); - } - else - { - prompt_I = d_Prompt->real(); - prompt_Q = d_Prompt->imag(); - } + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); } if (d_veml) { @@ -1249,20 +1231,6 @@ void dll_pll_veml_tracking::log_data(bool integrating) tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); - if (integrating) - { - //TODO: Improve this solution! - // It compensates the amplitude difference while integrating - if (d_extend_correlation_symbols_count > 0) - { - float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); - tmp_VE *= scale_factor; - tmp_E *= scale_factor; - tmp_P *= scale_factor; - tmp_L *= scale_factor; - tmp_VL *= scale_factor; - } - } try { @@ -1361,28 +1329,28 @@ int32_t dll_pll_veml_tracking::save_matfile() { return 1; } - auto abs_VE = std::vector(num_epoch); - auto abs_E = std::vector(num_epoch); - auto abs_P = std::vector(num_epoch); - auto abs_L = std::vector(num_epoch); - auto abs_VL = std::vector(num_epoch); - auto Prompt_I = std::vector(num_epoch); - auto Prompt_Q = std::vector(num_epoch); - auto PRN_start_sample_count = std::vector(num_epoch); - auto acc_carrier_phase_rad = std::vector(num_epoch); - auto carrier_doppler_hz = std::vector(num_epoch); - auto carrier_doppler_rate_hz = std::vector(num_epoch); - auto code_freq_chips = std::vector(num_epoch); - auto code_freq_rate_chips = std::vector(num_epoch); - auto carr_error_hz = std::vector(num_epoch); - auto carr_error_filt_hz = std::vector(num_epoch); - auto code_error_chips = std::vector(num_epoch); - auto code_error_filt_chips = std::vector(num_epoch); - auto CN0_SNV_dB_Hz = std::vector(num_epoch); - auto carrier_lock_test = std::vector(num_epoch); - auto aux1 = std::vector(num_epoch); - auto aux2 = std::vector(num_epoch); - auto PRN = std::vector(num_epoch); + auto *abs_VE = new float[num_epoch]; + auto *abs_E = new float[num_epoch]; + auto *abs_P = new float[num_epoch]; + auto *abs_L = new float[num_epoch]; + auto *abs_VL = new float[num_epoch]; + auto *Prompt_I = new float[num_epoch]; + auto *Prompt_Q = new float[num_epoch]; + auto *PRN_start_sample_count = new uint64_t[num_epoch]; + auto *acc_carrier_phase_rad = new float[num_epoch]; + auto *carrier_doppler_hz = new float[num_epoch]; + auto *carrier_doppler_rate_hz = new float[num_epoch]; + auto *code_freq_chips = new float[num_epoch]; + auto *code_freq_rate_chips = new float[num_epoch]; + auto *carr_error_hz = new float[num_epoch]; + auto *carr_error_filt_hz = new float[num_epoch]; + auto *code_error_chips = new float[num_epoch]; + auto *code_error_filt_chips = new float[num_epoch]; + auto *CN0_SNV_dB_Hz = new float[num_epoch]; + auto *carrier_lock_test = new float[num_epoch]; + auto *aux1 = new float[num_epoch]; + auto *aux2 = new double[num_epoch]; + auto *PRN = new uint32_t[num_epoch]; try { @@ -1419,6 +1387,28 @@ int32_t dll_pll_veml_tracking::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] carrier_doppler_rate_hz; + delete[] code_freq_chips; + delete[] code_freq_rate_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; return 1; } @@ -1432,95 +1422,117 @@ int32_t dll_pll_veml_tracking::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] carrier_doppler_rate_hz; + delete[] code_freq_chips; + delete[] code_freq_rate_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; return 0; } @@ -1557,21 +1569,18 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel) } } - void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { gr::thread::scoped_lock l(d_setlock); d_acquisition_gnss_synchro = p_gnss_synchro; } - void dll_pll_veml_tracking::stop_tracking() { gr::thread::scoped_lock l(d_setlock); d_state = 0; } - int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { @@ -1579,6 +1588,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) const auto *in = reinterpret_cast(input_items[0]); auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); + current_synchro_data.Flag_valid_symbol_output = false; if (d_pull_in_transitory == true) { @@ -1656,7 +1666,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) update_tracking_vars(); // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); + log_data(); if (!d_pull_in_transitory) { @@ -1678,42 +1688,18 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change { - float current_tracking_time_s = static_cast(d_sample_counter - d_acq_sample_stamp) / trk_parameters.fs_in; - if (current_tracking_time_s > 10) + //******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) { - d_symbol_history.push_back(d_Prompt->real()); - //******* preamble correlation ******** - int32_t corr_value = 0; - if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) + next_state = acquire_secondary(); + if (next_state) { - int i = 0; - for (const auto &iter : d_symbol_history) - { - if (iter < 0.0) // symbols clipping - { - corr_value -= d_preambles_symbols[i]; - } - else - { - corr_value += d_preambles_symbols[i]; - } - i++; - } - } - if (corr_value == d_preamble_length_symbols) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - next_state = true; } - else - { - next_state = false; - } - } - else - { - next_state = false; } } else @@ -1725,52 +1711,17 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { next_state = false; //keep in state 2 during pull-in transitory } - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - if (next_state) { // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); d_Prompt_circular_buffer.clear(); d_current_symbol = 0; + d_current_data_symbol = 0; if (d_enable_extended_integration) { @@ -1811,48 +1762,25 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } case 3: // coherent integration (correlation time extension) { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; // perform a correlation step do_correlation_step(in); - update_tracking_vars(); save_correlation_results(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) + update_tracking_vars(); + if (d_current_data_symbol == 0) { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; d_extend_correlation_symbols_count++; if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) { @@ -1863,9 +1791,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } case 4: // narrow tracking { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // perform a correlation step do_correlation_step(in); save_correlation_results(); @@ -1880,44 +1805,23 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { run_dll_pll(); update_tracking_vars(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) + if (d_current_data_symbol == 0) { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); + log_data(); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 170a3ddb5..4005cc88b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -85,7 +85,7 @@ private: void update_tracking_vars(); void clear_tracking_vars(); void save_correlation_results(); - void log_data(bool integrating); + void log_data(); int32_t save_matfile(); // tracking configuration vars @@ -97,22 +97,23 @@ private: // Signal parameters bool d_secondary; - bool interchange_iq; double d_signal_carrier_freq; double d_code_period; double d_code_chip_rate; uint32_t d_secondary_code_length; + uint32_t d_data_secondary_code_length; uint32_t d_code_length_chips; uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) int32_t d_symbols_per_bit; std::string systemName; std::string signal_type; std::string *d_secondary_code_string; + std::string *d_data_secondary_code_string; std::string signal_pretty_name; int32_t *d_preambles_symbols; int32_t d_preamble_length_symbols; - boost::circular_buffer d_symbol_history; + //boost::circular_buffer d_symbol_history; // dll filter buffer boost::circular_buffer d_dll_filt_history; @@ -144,6 +145,7 @@ private: bool d_enable_extended_integration; int32_t d_extend_correlation_symbols_count; int32_t d_current_symbol; + int32_t d_current_data_symbol; gr_complex d_VE_accu; gr_complex d_E_accu; @@ -152,6 +154,7 @@ private: gr_complex d_L_accu; gr_complex d_VL_accu; + gr_complex d_P_data_accu; gr_complex *d_Prompt_Data; double d_code_phase_step_chips; diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index 7ee713625..48dd4ea78 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -57,13 +57,17 @@ const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< beidou b1I chip period const int32_t BEIDOU_B1I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B1I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B1I_SECONDARY_CODE_STR = "00000100110101001110"; +const std::string BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR = {"1111110000001100001100"}; +const int32_t BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS = 22; + const std::string BEIDOU_B1I_D2_SECONDARY_CODE_STR = "00"; const int BEIDOU_B1I_PREAMBLE_LENGTH_BITS = 11; const int BEIDOU_B1I_PREAMBLE_LENGTH_SYMBOLS = 220; // ************** const double BEIDOU_B1I_PREAMBLE_DURATION_S = 0.220; const int BEIDOU_B1I_PREAMBLE_DURATION_MS = 220; -const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] -const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; // ************* +const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] +const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; +const int BEIDOU_B1I_TELEMETRY_SYMBOL_PERIOD_MS = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; const int BEIDOU_B1I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND * BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] const int BEIDOU_WORD_LENGTH = 4; //**************!< CRC + BEIDOU WORD (-2 -1 0 ... 29) Bits = 4 bytes const int BEIDOU_SUBFRAME_LENGTH = 40; //**************!< BEIDOU_WORD_LENGTH x 10 = 40 bytes diff --git a/src/core/system_parameters/Beidou_B3I.h b/src/core/system_parameters/Beidou_B3I.h index 13e4aa2ba..bd58aeae0 100644 --- a/src/core/system_parameters/Beidou_B3I.h +++ b/src/core/system_parameters/Beidou_B3I.h @@ -45,6 +45,8 @@ const uint32_t BEIDOU_B3I_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code perio const int32_t BEIDOU_B3I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B3I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B3I_SECONDARY_CODE_STR = "00000100110101001110"; +const std::string BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR = {"1111110000001100001100"}; +const int32_t BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS = 22; const std::string BEIDOU_B3I_D2_SECONDARY_CODE_STR = "00"; const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_BITS = 11; const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_SYMBOLS = 220; // ************** diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 44579c185..2c6c7465f 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -55,6 +55,7 @@ const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [c const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] const uint32_t GPS_L1_CA_CODE_PERIOD_MS = 1U; //!< GPS L1 C/A code period [ms] +const uint32_t GPS_L1_CA_BIT_PERIOD_MS = 20U; //!< GPS L1 C/A bit period [ms] const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds] //optimum parameters @@ -84,6 +85,7 @@ const int32_t GPS_L1_CA_HISTORY_DEEP = 100; 1, 0, 0, 0, 1, 0, 1, 1 \ } const std::string GPS_CA_PREAMBLE = {"10001011"}; +const std::string GPS_CA_PREAMBLE_SYMBOLS_STR = {"1111111111111111111100000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000001111111111111111111111111111111111111111"}; const int32_t GPS_CA_PREAMBLE_LENGTH_BITS = 8; const int32_t GPS_CA_PREAMBLE_LENGTH_SYMBOLS = 160; const double GPS_CA_PREAMBLE_DURATION_S = 0.160; From 587ec66e78fe298e96ad176bfe49ed2e336104f4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 1 Jul 2019 11:03:18 +0200 Subject: [PATCH 27/64] Remove wrong delete --- .../adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc | 6 +++--- .../tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc | 4 ++-- .../tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index 447e22231..a8c979425 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -271,7 +271,7 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( } } - delete[] ca_codes_f; + volk_gnsssdr_free(ca_codes_f); if (d_track_pilot) { volk_gnsssdr_free(data_codes_f); @@ -289,10 +289,10 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( GalileoE1DllPllVemlTrackingFpga::~GalileoE1DllPllVemlTrackingFpga() { - delete[] d_ca_codes; + volk_gnsssdr_free(d_ca_codes); if (d_track_pilot) { - delete[] d_data_codes; + volk_gnsssdr_free(d_data_codes); } } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 7e3874101..cf39be1f1 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -273,10 +273,10 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( GalileoE5aDllPllTrackingFpga::~GalileoE5aDllPllTrackingFpga() { - delete[] d_ca_codes; + volk_gnsssdr_free(d_ca_codes); if (d_track_pilot) { - delete[] d_data_codes; + volk_gnsssdr_free(d_data_codes); } } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 2d244fbc0..0995f3253 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -239,7 +239,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( GpsL1CaDllPllTrackingFpga::~GpsL1CaDllPllTrackingFpga() { - delete[] d_ca_codes; + volk_gnsssdr_free(d_ca_codes); } From 658b001accdacf635bb40bf73d56f0e8265fa2f1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 1 Jul 2019 11:44:04 +0200 Subject: [PATCH 28/64] Fix building --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 220 ++++++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 3 +- 2 files changed, 93 insertions(+), 130 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index c90301a33..1a03f987d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -63,9 +63,11 @@ #include // for fill_n #include // for fmod, round, floor #include // for exception -#include // for cout, cerr +#include +#include // for cout, cerr #include #include +#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -152,7 +154,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); - //GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required + // GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; d_code_samples_per_chip = 1; @@ -173,19 +175,19 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = true; if (trk_parameters.track_pilot) { - //synchronize pilot secondary code + // synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); - //remove data secondary code - //remove Neuman-Hofman Code (see IS-GPS-705D) + // remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; } else { - //synchronize and remove data secondary code - //remove Neuman-Hofman Code (see IS-GPS-705D) + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; @@ -213,7 +215,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GALILEO_E1_CODE_PERIOD; d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ; d_code_length_chips = static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS); - //Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required + // Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required d_symbols_per_bit = 1; d_correlation_length_ms = 4; d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip @@ -244,16 +246,16 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = true; if (trk_parameters.track_pilot) { - //synchronize pilot secondary code + // synchronize pilot secondary code d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; - //remove data secondary code + // remove data secondary code d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); } else { - //synchronize and remove data secondary code + // synchronize and remove data secondary code d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); signal_pretty_name = signal_pretty_name + "I"; @@ -287,7 +289,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; - //synchronize and remove data secondary code + // synchronize and remove data secondary code d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); @@ -540,7 +542,7 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) switch (tlm_event) { - case 1: //tlm fault in current channel + case 1: // tlm fault in current channel { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); @@ -565,7 +567,7 @@ void dll_pll_veml_tracking::start_tracking() { gr::thread::scoped_lock l(d_setlock); - // correct the code phase according to the delay between acq and trk + // correct the code phase according to the delay between acq and trk d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; @@ -575,55 +577,58 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_phase_rate_step_rad = 0.0; d_carr_ph_history.clear(); d_code_ph_history.clear(); + std::array Signal_; + std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); if (systemName == "GPS" and signal_type == "1C") { - gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); } else if (systemName == "GPS" and signal_type == "2S") { - gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l2c_m_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); } else if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) { - gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); - gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); + gps_l5q_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "1B") { if (trk_parameters.track_pilot) { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); - galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + std::array pilot_signal = {{'1', 'C', '\0'}}; + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "5X") { auto *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); - galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); + std::array signal_type_ = {{'5', 'X', '\0'}}; + galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_); if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); for (uint32_t i = 0; i < d_code_length_chips; i++) { d_tracking_code[i] = aux_code[i].imag(); - d_data_code[i] = aux_code[i].real(); //the same because it is generated the full signal (E5aI + E5aQ) + d_data_code[i] = aux_code[i].real(); // the same because it is generated the full signal (E5aI + E5aQ) } d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); @@ -639,7 +644,7 @@ void dll_pll_veml_tracking::start_tracking() } else if (systemName == "Beidou" and signal_type == "B1") { - beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -658,7 +663,7 @@ void dll_pll_veml_tracking::start_tracking() else if (systemName == "Beidou" and signal_type == "B3") { - beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + beidou_b3i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -830,11 +835,9 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); // Carrier lock indicator d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); - //d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); // Loss of lock detection if (!d_pull_in_transitory) { - //d_carrier_lock_test < d_carrier_lock_threshold or if (d_carrier_lock_test < d_carrier_lock_threshold) { d_carrier_lock_fail_counter++; @@ -925,20 +928,20 @@ void dll_pll_veml_tracking::run_dll_pll() if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) { // FLL discriminator - //d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + // d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; d_P_accu_old = d_P_accu; - //std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; + // std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; // Carrier discriminator filter if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) { - //pure FLL, disable PLL + // pure FLL, disable PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); } else { - //FLL-aided PLL + // FLL-aided PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); } } @@ -953,6 +956,7 @@ void dll_pll_veml_tracking::run_dll_pll() // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; + // ################## DLL ########################################################## // DLL discriminator if (d_veml) @@ -1021,7 +1025,6 @@ void dll_pll_veml_tracking::update_tracking_vars() // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation T_prn_samples = T_prn_seconds * trk_parameters.fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - //d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples d_current_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples //################### PLL COMMANDS ################################################# @@ -1047,15 +1050,15 @@ void dll_pll_veml_tracking::update_tracking_vars() d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; + // std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); // carrier phase accumulator - //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); - //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); - //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; + // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + // std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); //################### DLL COMMANDS ################################################# @@ -1128,7 +1131,7 @@ void dll_pll_veml_tracking::save_correlation_results() d_L_accu += *d_Late; } - //data secondary code roll-up + // data secondary code roll-up if (d_symbols_per_bit > 1) { if (d_data_secondary_code_length > 0) @@ -1155,7 +1158,7 @@ void dll_pll_veml_tracking::save_correlation_results() d_P_data_accu -= *d_Prompt; } } - //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + // std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; d_current_data_symbol++; // data secondary code roll-up d_current_data_symbol %= d_data_secondary_code_length; @@ -1197,6 +1200,7 @@ void dll_pll_veml_tracking::save_correlation_results() } } + void dll_pll_veml_tracking::log_data() { if (d_dump) @@ -1329,29 +1333,28 @@ int32_t dll_pll_veml_tracking::save_matfile() { return 1; } - auto *abs_VE = new float[num_epoch]; - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *abs_VL = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new float[num_epoch]; - auto *carrier_doppler_hz = new float[num_epoch]; - auto *carrier_doppler_rate_hz = new float[num_epoch]; - auto *code_freq_chips = new float[num_epoch]; - auto *code_freq_rate_chips = new float[num_epoch]; - auto *carr_error_hz = new float[num_epoch]; - auto *carr_error_filt_hz = new float[num_epoch]; - auto *code_error_chips = new float[num_epoch]; - auto *code_error_filt_chips = new float[num_epoch]; - auto *CN0_SNV_dB_Hz = new float[num_epoch]; - auto *carrier_lock_test = new float[num_epoch]; - auto *aux1 = new float[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; - + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_doppler_rate_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto code_freq_rate_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { if (dump_file.is_open()) @@ -1387,28 +1390,6 @@ int32_t dll_pll_veml_tracking::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -1422,117 +1403,95 @@ int32_t dll_pll_veml_tracking::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } @@ -1569,18 +1528,21 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel) } } + void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { gr::thread::scoped_lock l(d_setlock); d_acquisition_gnss_synchro = p_gnss_synchro; } + void dll_pll_veml_tracking::stop_tracking() { gr::thread::scoped_lock l(d_setlock); d_state = 0; } + int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 4005cc88b..d565e8fa1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -113,7 +113,6 @@ private: int32_t *d_preambles_symbols; int32_t d_preamble_length_symbols; - //boost::circular_buffer d_symbol_history; // dll filter buffer boost::circular_buffer d_dll_filt_history; @@ -130,6 +129,7 @@ private: float *d_prompt_data_shift; Cpu_Multicorrelator_Real_Codes multicorrelator_cpu; Cpu_Multicorrelator_Real_Codes correlator_data_cpu; //for data channel + /* TODO: currently the multicorrelator does not support adding extra correlator with different local code, thus we need extra multicorrelator instance. Implement this functionality inside multicorrelator class @@ -163,6 +163,7 @@ private: double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; boost::circular_buffer> d_carr_ph_history; + // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; float d_rem_carr_phase_rad; From 54553a8cffb7b5d0692919e752e7b65f030f5e4c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 1 Jul 2019 15:26:48 +0200 Subject: [PATCH 29/64] Remove delete --- src/core/libs/gnss_sdr_supl_client.cc | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index fb429b7f4..1729583cd 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -39,10 +39,10 @@ #include #include #include // for pow -#include // for strcpy #include // for exception #include // for cerr #include // for pair +#include Gnss_Sdr_Supl_Client::Gnss_Sdr_Supl_Client() { @@ -173,14 +173,16 @@ int Gnss_Sdr_Supl_Client::get_assistance(int i_mcc, int i_mns, int i_lac, int i_ supl_set_gsm_cell(&ctx, mcc, mns, lac, ci); // PERFORM SUPL COMMUNICATION - char* cstr = new char[server_name.length() + 1]; - strcpy(cstr, server_name.c_str()); + std::vector cstr(server_name.length() + 1); + for (int i = 0; i != server_name.length(); ++i) + { + cstr[i] = static_cast(server_name[i]); + } int err; ctx.p.request = request; // select assistance info request from a pre-defined set - //std::cout<<"mcc="< Date: Mon, 1 Jul 2019 21:54:52 +0200 Subject: [PATCH 30/64] Improve memory management In class definitions, first write the public interface, then private --- ...eo_e5a_noncoherent_iq_acquisition_caf_cc.h | 166 ++++++------ .../galileo_pcps_8ms_acquisition_cc.h | 122 ++++----- .../gnuradio_blocks/pcps_acquisition.cc | 124 +++------ .../gnuradio_blocks/pcps_acquisition.h | 138 +++++----- .../pcps_acquisition_fine_doppler_cc.cc | 44 +-- .../pcps_acquisition_fine_doppler_cc.h | 136 +++++----- .../gnuradio_blocks/pcps_acquisition_fpga.cc | 3 +- .../gnuradio_blocks/pcps_acquisition_fpga.h | 81 +++--- .../pcps_assisted_acquisition_cc.cc | 37 +-- .../pcps_assisted_acquisition_cc.h | 129 +++++---- .../pcps_cccwsr_acquisition_cc.h | 109 ++++---- .../pcps_opencl_acquisition_cc.h | 252 +++++++++--------- .../pcps_quicksync_acquisition_cc.h | 134 +++++----- .../pcps_tong_acquisition_cc.h | 111 ++++---- 14 files changed, 734 insertions(+), 852 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h index 7b6b9925f..3cc43bebb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h @@ -52,8 +52,8 @@ class galileo_e5a_noncoherentIQ_acquisition_caf_cc; using galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr = boost::shared_ptr; -galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr -galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(unsigned int sampled_ms, +galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr galileo_e5a_noncoherentIQ_make_acquisition_caf_cc( + unsigned int sampled_ms, unsigned int max_dwells, unsigned int doppler_max, int64_t fs_in, int samples_per_ms, int samples_per_code, @@ -72,88 +72,6 @@ galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(unsigned int sampled_ms, */ class galileo_e5a_noncoherentIQ_acquisition_caf_cc : public gr::block { -private: - friend galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr - galileo_e5a_noncoherentIQ_make_acquisition_caf_cc( - unsigned int sampled_ms, - unsigned int max_dwells, - unsigned int doppler_max, int64_t fs_in, - int samples_per_ms, int samples_per_code, - bool bit_transition_flag, - bool dump, - std::string dump_filename, - bool both_signal_components_, - int CAF_window_hz_, - int Zero_padding_); - - galileo_e5a_noncoherentIQ_acquisition_caf_cc( - unsigned int sampled_ms, - unsigned int max_dwells, - unsigned int doppler_max, int64_t fs_in, - int samples_per_ms, int samples_per_code, - bool bit_transition_flag, - bool dump, - std::string dump_filename, - bool both_signal_components_, - int CAF_window_hz_, - int Zero_padding_); - - void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, - int doppler_offset); - float estimate_input_power(gr_complex* in); - - std::weak_ptr d_channel_fsm; - int64_t d_fs_in; - int d_samples_per_ms; - int d_sampled_ms; - int d_samples_per_code; - unsigned int d_doppler_resolution; - float d_threshold; - std::string d_satellite_str; - unsigned int d_doppler_max; - unsigned int d_doppler_step; - unsigned int d_max_dwells; - unsigned int d_well_count; - unsigned int d_fft_size; - uint64_t d_sample_counter; - gr_complex** d_grid_doppler_wipeoffs; - unsigned int d_num_doppler_bins; - gr_complex* d_fft_code_I_A; - gr_complex* d_fft_code_I_B; - gr_complex* d_fft_code_Q_A; - gr_complex* d_fft_code_Q_B; - gr_complex* d_inbuffer; - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - unsigned int d_code_phase; - float d_doppler_freq; - float d_mag; - float* d_magnitudeIA; - float* d_magnitudeIB; - float* d_magnitudeQA; - float* d_magnitudeQB; - float d_input_power; - float d_test_statistics; - bool d_bit_transition_flag; - std::ofstream d_dump_file; - bool d_active; - int d_state; - bool d_dump; - bool d_both_signal_components; - // bool d_CAF_filter; - int d_CAF_window_hz; - float* d_CAF_vector; - float* d_CAF_vector_I; - float* d_CAF_vector_Q; - // double* d_CAF_vector; - // double* d_CAF_vector_I; - // double* d_CAF_vector_Q; - unsigned int d_channel; - std::string d_dump_filename; - unsigned int d_buffer_count; - unsigned int d_gr_stream_buffer; - public: /*! * \brief Default destructor. @@ -257,5 +175,85 @@ public: int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + +private: + friend galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr + galileo_e5a_noncoherentIQ_make_acquisition_caf_cc( + unsigned int sampled_ms, + unsigned int max_dwells, + unsigned int doppler_max, int64_t fs_in, + int samples_per_ms, int samples_per_code, + bool bit_transition_flag, + bool dump, + std::string dump_filename, + bool both_signal_components_, + int CAF_window_hz_, + int Zero_padding_); + + galileo_e5a_noncoherentIQ_acquisition_caf_cc( + unsigned int sampled_ms, + unsigned int max_dwells, + unsigned int doppler_max, int64_t fs_in, + int samples_per_ms, int samples_per_code, + bool bit_transition_flag, + bool dump, + std::string dump_filename, + bool both_signal_components_, + int CAF_window_hz_, + int Zero_padding_); + + void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, + int doppler_offset); + + float estimate_input_power(gr_complex* in); + + std::weak_ptr d_channel_fsm; + int64_t d_fs_in; + int d_samples_per_ms; + int d_sampled_ms; + int d_samples_per_code; + unsigned int d_doppler_resolution; + float d_threshold; + std::string d_satellite_str; + unsigned int d_doppler_max; + unsigned int d_doppler_step; + unsigned int d_max_dwells; + unsigned int d_well_count; + unsigned int d_fft_size; + uint64_t d_sample_counter; + gr_complex** d_grid_doppler_wipeoffs; + unsigned int d_num_doppler_bins; + gr_complex* d_fft_code_I_A; + gr_complex* d_fft_code_I_B; + gr_complex* d_fft_code_Q_A; + gr_complex* d_fft_code_Q_B; + gr_complex* d_inbuffer; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + unsigned int d_code_phase; + float d_doppler_freq; + float d_mag; + float* d_magnitudeIA; + float* d_magnitudeIB; + float* d_magnitudeQA; + float* d_magnitudeQB; + float d_input_power; + float d_test_statistics; + bool d_bit_transition_flag; + std::ofstream d_dump_file; + bool d_active; + int d_state; + bool d_dump; + bool d_both_signal_components; + int d_CAF_window_hz; + float* d_CAF_vector; + float* d_CAF_vector_I; + float* d_CAF_vector_Q; + unsigned int d_channel; + std::string d_dump_filename; + unsigned int d_buffer_count; + unsigned int d_gr_stream_buffer; }; + #endif /* GALILEO_E5A_NONCOHERENT_IQ_ACQUISITION_CAF_CC_H_ */ diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h index 1dd5b5e78..36dd9d7fe 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h @@ -62,67 +62,6 @@ galileo_pcps_8ms_make_acquisition_cc(uint32_t sampled_ms, */ class galileo_pcps_8ms_acquisition_cc : public gr::block { -private: - friend galileo_pcps_8ms_acquisition_cc_sptr - galileo_pcps_8ms_make_acquisition_cc( - uint32_t sampled_ms, - uint32_t max_dwells, - uint32_t doppler_max, - int64_t fs_in, - int32_t samples_per_ms, - int32_t samples_per_code, - bool dump, - std::string dump_filename); - - galileo_pcps_8ms_acquisition_cc( - uint32_t sampled_ms, - uint32_t max_dwells, - uint32_t doppler_max, - int64_t fs_in, - int32_t samples_per_ms, - int32_t samples_per_code, - bool dump, - std::string dump_filename); - - void calculate_magnitudes( - gr_complex* fft_begin, - int32_t doppler_shift, - int32_t doppler_offset); - - int64_t d_fs_in; - int32_t d_samples_per_ms; - int32_t d_samples_per_code; - uint32_t d_doppler_resolution; - float d_threshold; - std::string d_satellite_str; - uint32_t d_doppler_max; - uint32_t d_doppler_step; - uint32_t d_sampled_ms; - uint32_t d_max_dwells; - uint32_t d_well_count; - uint32_t d_fft_size; - uint64_t d_sample_counter; - gr_complex** d_grid_doppler_wipeoffs; - uint32_t d_num_doppler_bins; - gr_complex* d_fft_code_A; - gr_complex* d_fft_code_B; - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - uint32_t d_code_phase; - float d_doppler_freq; - float d_mag; - float* d_magnitude; - float d_input_power; - float d_test_statistics; - std::ofstream d_dump_file; - bool d_active; - int32_t d_state; - bool d_dump; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - std::string d_dump_filename; - public: /*! * \brief Default destructor. @@ -226,6 +165,67 @@ public: int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + +private: + friend galileo_pcps_8ms_acquisition_cc_sptr + galileo_pcps_8ms_make_acquisition_cc( + uint32_t sampled_ms, + uint32_t max_dwells, + uint32_t doppler_max, + int64_t fs_in, + int32_t samples_per_ms, + int32_t samples_per_code, + bool dump, + std::string dump_filename); + + galileo_pcps_8ms_acquisition_cc( + uint32_t sampled_ms, + uint32_t max_dwells, + uint32_t doppler_max, + int64_t fs_in, + int32_t samples_per_ms, + int32_t samples_per_code, + bool dump, + std::string dump_filename); + + void calculate_magnitudes( + gr_complex* fft_begin, + int32_t doppler_shift, + int32_t doppler_offset); + + int64_t d_fs_in; + int32_t d_samples_per_ms; + int32_t d_samples_per_code; + uint32_t d_doppler_resolution; + float d_threshold; + std::string d_satellite_str; + uint32_t d_doppler_max; + uint32_t d_doppler_step; + uint32_t d_sampled_ms; + uint32_t d_max_dwells; + uint32_t d_well_count; + uint32_t d_fft_size; + uint64_t d_sample_counter; + gr_complex** d_grid_doppler_wipeoffs; + uint32_t d_num_doppler_bins; + gr_complex* d_fft_code_A; + gr_complex* d_fft_code_B; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + uint32_t d_code_phase; + float d_doppler_freq; + float d_mag; + float* d_magnitude; + float d_input_power; + float d_test_statistics; + std::ofstream d_dump_file; + bool d_active; + int32_t d_state; + bool d_dump; + uint32_t d_channel; + std::weak_ptr d_channel_fsm; + std::string d_dump_filename; }; #endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 39270efe3..4947b9d2f 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -134,10 +134,9 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu acq_parameters.max_dwells = 1; // Activation of acq_parameters.bit_transition_flag invalidates the value of acq_parameters.max_dwells } - d_tmp_buffer = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); - d_fft_codes = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_magnitude = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); - d_input_signal = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_tmp_buffer = std::vector(d_fft_size); + d_fft_codes = std::vector>(d_fft_size); + d_input_signal = std::vector>(d_fft_size); // Direct FFT d_fft_if = std::make_shared(d_fft_size, true); @@ -146,18 +145,11 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu d_ifft = std::make_shared(d_fft_size, false); d_gnss_synchro = nullptr; - d_grid_doppler_wipeoffs = nullptr; - d_grid_doppler_wipeoffs_step_two = nullptr; - d_magnitude_grid = nullptr; d_worker_active = false; - d_data_buffer = static_cast(volk_gnsssdr_malloc(d_consumed_samples * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_data_buffer = std::vector>(d_consumed_samples); if (d_cshort) { - d_data_buffer_sc = static_cast(volk_gnsssdr_malloc(d_consumed_samples * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment())); - } - else - { - d_data_buffer_sc = nullptr; + d_data_buffer_sc = std::vector(d_consumed_samples); } grid_ = arma::fmat(); narrow_grid_ = arma::fmat(); @@ -213,36 +205,7 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu } -pcps_acquisition::~pcps_acquisition() -{ - 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_magnitude_grid[i]); - } - delete[] d_grid_doppler_wipeoffs; - delete[] d_magnitude_grid; - } - if (acq_parameters.make_2_steps) - { - for (uint32_t i = 0; i < d_num_doppler_bins_step2; i++) - { - volk_gnsssdr_free(d_grid_doppler_wipeoffs_step_two[i]); - } - delete[] d_grid_doppler_wipeoffs_step_two; - } - volk_gnsssdr_free(d_fft_codes); - volk_gnsssdr_free(d_magnitude); - volk_gnsssdr_free(d_tmp_buffer); - volk_gnsssdr_free(d_input_signal); - volk_gnsssdr_free(d_data_buffer); - if (d_cshort) - { - volk_gnsssdr_free(d_data_buffer_sc); - } -} +pcps_acquisition::~pcps_acquisition() = default; void pcps_acquisition::set_resampler_latency(uint32_t latency_samples) @@ -286,7 +249,7 @@ void pcps_acquisition::set_local_code(std::complex* code) } d_fft_if->execute(); // We need the FFT of local code - volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); + volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size); } @@ -309,7 +272,7 @@ bool pcps_acquisition::is_fdma() } -void pcps_acquisition::update_local_carrier(gr_complex* carrier_vector, int32_t correlator_length_samples, float freq) +void pcps_acquisition::update_local_carrier(gsl::span carrier_vector, float freq) { float phase_step_rad; if (acq_parameters.use_automatic_resampler) @@ -322,7 +285,7 @@ void pcps_acquisition::update_local_carrier(gr_complex* carrier_vector, int32_t } float _phase[1]; _phase[0] = 0.0; - volk_gnsssdr_s32f_sincos_32fc(carrier_vector, -phase_step_rad, _phase, correlator_length_samples); + volk_gnsssdr_s32f_sincos_32fc(carrier_vector.data(), -phase_step_rad, _phase, carrier_vector.length()); } @@ -342,27 +305,18 @@ void pcps_acquisition::init() d_num_doppler_bins = static_cast(std::ceil(static_cast(static_cast(acq_parameters.doppler_max) - static_cast(-acq_parameters.doppler_max)) / static_cast(d_doppler_step))); // Create the carrier Doppler wipeoff signals - if (d_grid_doppler_wipeoffs == nullptr) + if (d_grid_doppler_wipeoffs.empty()) { - d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; + d_grid_doppler_wipeoffs = std::vector>>(d_num_doppler_bins, std::vector>(d_fft_size)); } - if (acq_parameters.make_2_steps && (d_grid_doppler_wipeoffs_step_two == nullptr)) + if (acq_parameters.make_2_steps && (d_grid_doppler_wipeoffs_step_two.empty())) { - d_grid_doppler_wipeoffs_step_two = new gr_complex*[d_num_doppler_bins_step2]; - for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) - { - d_grid_doppler_wipeoffs_step_two[doppler_index] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - } + d_grid_doppler_wipeoffs_step_two = std::vector>>(d_num_doppler_bins_step2, std::vector>(d_fft_size)); } - if (d_magnitude_grid == nullptr) + if (d_magnitude_grid.empty()) { - d_magnitude_grid = new float*[d_num_doppler_bins]; - for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) - { - d_grid_doppler_wipeoffs[doppler_index] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_magnitude_grid[doppler_index] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); - } + d_magnitude_grid = std::vector>(d_num_doppler_bins, std::vector(d_fft_size)); } for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) @@ -372,7 +326,7 @@ void pcps_acquisition::init() d_magnitude_grid[doppler_index][k] = 0.0; } int32_t doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * doppler_index; - update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_old_freq + doppler); + update_local_carrier(gsl::span(d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size), d_old_freq + doppler); } d_worker_active = false; @@ -391,7 +345,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs() for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) { int32_t doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * doppler_index; - update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_old_freq + doppler); + update_local_carrier(gsl::span(d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size), d_old_freq + doppler); } } @@ -401,7 +355,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs_step2() for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) { float doppler = (static_cast(doppler_index) - static_cast(floor(d_num_doppler_bins_step2 / 2.0))) * acq_parameters.doppler_step2; - update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], d_fft_size, d_doppler_center_step_two + doppler); + update_local_carrier(gsl::span(d_grid_doppler_wipeoffs_step_two[doppler_index].data(), d_fft_size), d_doppler_center_step_two + doppler); } } @@ -590,7 +544,7 @@ float pcps_acquisition::max_to_input_power_statistic(uint32_t& indext, int32_t& // Find the correlation peak and the carrier frequency for (uint32_t i = 0; i < num_doppler_bins; i++) { - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i].data(), d_fft_size); if (d_magnitude_grid[i][tmp_intex_t] > grid_maximum) { grid_maximum = d_magnitude_grid[i][tmp_intex_t]; @@ -627,7 +581,7 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int32_t // Find the correlation peak and the carrier frequency for (uint32_t i = 0; i < num_doppler_bins; i++) { - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i].data(), d_fft_size); if (d_magnitude_grid[i][tmp_intex_t] > firstPeak) { firstPeak = d_magnitude_grid[i][tmp_intex_t]; @@ -661,7 +615,7 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int32_t } int32_t idx = excludeRangeIndex1; - memcpy(d_tmp_buffer, d_magnitude_grid[index_doppler], d_fft_size); + memcpy(d_tmp_buffer.data(), d_magnitude_grid[index_doppler].data(), d_fft_size); do { d_tmp_buffer[idx] = 0.0; @@ -674,7 +628,7 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int32_t while (idx != excludeRangeIndex2); // Find the second highest correlation peak in the same freq. bin --- - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_tmp_buffer, d_fft_size); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_tmp_buffer.data(), d_fft_size); float secondPeak = d_tmp_buffer[tmp_intex_t]; // Compute the test statistics and compare to the threshold @@ -692,9 +646,9 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) int32_t effective_fft_size = (acq_parameters.bit_transition_flag ? d_fft_size / 2 : d_fft_size); if (d_cshort) { - volk_gnsssdr_16ic_convert_32fc(d_data_buffer, d_data_buffer_sc, d_consumed_samples); + volk_gnsssdr_16ic_convert_32fc(d_data_buffer.data(), d_data_buffer_sc.data(), d_consumed_samples); } - memcpy(d_input_signal, d_data_buffer, d_consumed_samples * sizeof(gr_complex)); + memcpy(d_input_signal.data(), d_data_buffer.data(), d_consumed_samples * sizeof(gr_complex)); if (d_fft_size > d_consumed_samples) { for (uint32_t i = d_consumed_samples; i < d_fft_size; i++) @@ -702,7 +656,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) d_input_signal[i] = gr_complex(0.0, 0.0); } } - const gr_complex* in = d_input_signal; // Get the input samples pointer + const gr_complex* in = d_input_signal.data(); // Get the input samples pointer d_input_power = 0.0; d_mag = 0.0; @@ -720,8 +674,8 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) if (d_use_CFAR_algorithm_flag or acq_parameters.bit_transition_flag) { // Compute the input signal power estimation - volk_32fc_magnitude_squared_32f(d_tmp_buffer, in, d_fft_size); - volk_32f_accumulator_s32f(&d_input_power, d_tmp_buffer, d_fft_size); + volk_32fc_magnitude_squared_32f(d_tmp_buffer.data(), in, d_fft_size); + volk_32f_accumulator_s32f(&d_input_power, d_tmp_buffer.data(), d_fft_size); d_input_power /= static_cast(d_fft_size); } @@ -731,14 +685,14 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) { // Remove Doppler - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size); // Perform the FFT-based convolution (parallel time search) // Compute the FFT of the carrier wiped--off incoming signal d_fft_if->execute(); // Multiply carrier wiped--off, Fourier transformed incoming signal with the local FFT'd code reference - 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 d_ifft->execute(); @@ -747,17 +701,17 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) size_t offset = (acq_parameters.bit_transition_flag ? effective_fft_size : 0); if (d_num_noncoherent_integrations_counter == 1) { - volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index], d_ifft->get_outbuf() + offset, effective_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, effective_fft_size); } else { - volk_32fc_magnitude_squared_32f(d_tmp_buffer, d_ifft->get_outbuf() + offset, effective_fft_size); - volk_32f_x2_add_32f(d_magnitude_grid[doppler_index], d_magnitude_grid[doppler_index], d_tmp_buffer, effective_fft_size); + volk_32fc_magnitude_squared_32f(d_tmp_buffer.data(), d_ifft->get_outbuf() + offset, effective_fft_size); + volk_32f_x2_add_32f(d_magnitude_grid[doppler_index].data(), d_magnitude_grid[doppler_index].data(), d_tmp_buffer.data(), effective_fft_size); } // Record results to file if required if (d_dump and d_channel == d_dump_channel) { - memcpy(grid_.colptr(doppler_index), d_magnitude_grid[doppler_index], sizeof(float) * effective_fft_size); + memcpy(grid_.colptr(doppler_index), d_magnitude_grid[doppler_index].data(), sizeof(float) * effective_fft_size); } } @@ -789,7 +743,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) { for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) { - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs_step_two[doppler_index], d_fft_size); + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs_step_two[doppler_index].data(), d_fft_size); // Perform the FFT-based convolution (parallel time search) // Compute the FFT of the carrier wiped--off incoming signal @@ -797,7 +751,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) // Multiply carrier wiped--off, Fourier transformed incoming signal // with the local FFT'd code reference using SIMD operations with VOLK library - volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, 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 d_ifft->execute(); @@ -805,17 +759,17 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count) size_t offset = (acq_parameters.bit_transition_flag ? effective_fft_size : 0); if (d_num_noncoherent_integrations_counter == 1) { - volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index], d_ifft->get_outbuf() + offset, effective_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, effective_fft_size); } else { - volk_32fc_magnitude_squared_32f(d_tmp_buffer, d_ifft->get_outbuf() + offset, effective_fft_size); - volk_32f_x2_add_32f(d_magnitude_grid[doppler_index], d_magnitude_grid[doppler_index], d_tmp_buffer, effective_fft_size); + volk_32fc_magnitude_squared_32f(d_tmp_buffer.data(), d_ifft->get_outbuf() + offset, effective_fft_size); + volk_32f_x2_add_32f(d_magnitude_grid[doppler_index].data(), d_magnitude_grid[doppler_index].data(), d_tmp_buffer.data(), effective_fft_size); } // Record results to file if required if (d_dump and d_channel == d_dump_channel) { - memcpy(narrow_grid_.colptr(doppler_index), d_magnitude_grid[doppler_index], sizeof(float) * effective_fft_size); + memcpy(narrow_grid_.colptr(doppler_index), d_magnitude_grid[doppler_index].data(), sizeof(float) * effective_fft_size); } } // Compute the test statistic diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h index b7b27c70c..cfdf5c103 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h @@ -61,18 +61,25 @@ #include // for scoped_lock #include // for gr_vector_const_void_star #include // for lv_16sc_t +#include #include #include #include #include +#include +#if HAS_SPAN +#include +namespace gsl = std; +#else +#include +#endif class Gnss_Synchro; class pcps_acquisition; using pcps_acquisition_sptr = boost::shared_ptr; -pcps_acquisition_sptr -pcps_make_acquisition(const Acq_Conf& conf_); +pcps_acquisition_sptr pcps_make_acquisition(const Acq_Conf& conf_); /*! * \brief This class implements a Parallel Code Phase Search Acquisition. @@ -82,75 +89,6 @@ pcps_make_acquisition(const Acq_Conf& conf_); */ class pcps_acquisition : public gr::block { -private: - friend pcps_acquisition_sptr - pcps_make_acquisition(const Acq_Conf& conf_); - - pcps_acquisition(const Acq_Conf& conf_); - - void update_local_carrier(gr_complex* carrier_vector, int32_t correlator_length_samples, float freq); - void update_grid_doppler_wipeoffs(); - void update_grid_doppler_wipeoffs_step2(); - bool is_fdma(); - - void acquisition_core(uint64_t samp_count); - - void send_negative_acquisition(); - - void send_positive_acquisition(); - - void dump_results(int32_t effective_fft_size); - - float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); - float max_to_input_power_statistic(uint32_t& indext, int32_t& doppler, float input_power, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); - - bool start(); - - - Acq_Conf acq_parameters; - bool d_active; - bool d_worker_active; - bool d_cshort; - bool d_step_two; - bool d_use_CFAR_algorithm_flag; - int32_t d_positive_acq; - float d_threshold; - float d_mag; - float d_input_power; - float d_test_statistics; - float* d_magnitude; - float** d_magnitude_grid; - float* d_tmp_buffer; - gr_complex* d_input_signal; - uint32_t d_samplesPerChip; - int64_t d_old_freq; - int32_t d_state; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - uint32_t d_doppler_step; - float d_doppler_center_step_two; - uint32_t d_num_noncoherent_integrations_counter; - uint32_t d_fft_size; - uint32_t d_consumed_samples; - uint32_t d_num_doppler_bins; - uint64_t d_sample_counter; - gr_complex** d_grid_doppler_wipeoffs; - gr_complex** d_grid_doppler_wipeoffs_step_two; - gr_complex* d_fft_codes; - gr_complex* d_data_buffer; - lv_16sc_t* d_data_buffer_sc; - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - arma::fmat grid_; - arma::fmat narrow_grid_; - uint32_t d_num_doppler_bins_step2; - int64_t d_dump_number; - uint32_t d_dump_channel; - uint32_t d_buffer_count; - bool d_dump; - std::string d_dump_filename; - public: ~pcps_acquisition(); @@ -250,7 +188,6 @@ public: d_doppler_step = doppler_step; } - void set_resampler_latency(uint32_t latency_samples); /*! @@ -259,6 +196,63 @@ public: int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + +private: + friend pcps_acquisition_sptr pcps_make_acquisition(const Acq_Conf& conf_); + pcps_acquisition(const Acq_Conf& conf_); + bool d_active; + bool d_worker_active; + bool d_cshort; + bool d_step_two; + bool d_use_CFAR_algorithm_flag; + bool d_dump; + int32_t d_state; + int32_t d_positive_acq; + uint32_t d_channel; + uint32_t d_samplesPerChip; + uint32_t d_doppler_step; + uint32_t d_num_noncoherent_integrations_counter; + uint32_t d_fft_size; + uint32_t d_consumed_samples; + uint32_t d_num_doppler_bins; + uint32_t d_num_doppler_bins_step2; + uint32_t d_dump_channel; + uint32_t d_buffer_count; + uint64_t d_sample_counter; + int64_t d_dump_number; + int64_t d_old_freq; + float d_threshold; + float d_mag; + float d_input_power; + float d_test_statistics; + float d_doppler_center_step_two; + std::string d_dump_filename; + std::vector> d_magnitude_grid; + std::vector d_tmp_buffer; + std::vector> d_input_signal; + std::vector>> d_grid_doppler_wipeoffs; + std::vector>> d_grid_doppler_wipeoffs_step_two; + std::vector> d_fft_codes; + std::vector> d_data_buffer; + std::vector d_data_buffer_sc; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + std::weak_ptr d_channel_fsm; + Acq_Conf acq_parameters; + Gnss_Synchro* d_gnss_synchro; + arma::fmat grid_; + arma::fmat narrow_grid_; + void update_local_carrier(gsl::span carrier_vector, float freq); + void update_grid_doppler_wipeoffs(); + void update_grid_doppler_wipeoffs_step2(); + void acquisition_core(uint64_t samp_count); + void send_negative_acquisition(); + void send_positive_acquisition(); + void dump_results(int32_t effective_fft_size); + bool is_fdma(); + bool start(); + float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); + float max_to_input_power_statistic(uint32_t& indext, int32_t& doppler, float input_power, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); }; #endif /* GNSS_SDR_PCPS_ACQUISITION_H_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index 1a3642af9..44f262388 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -138,8 +138,6 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_threshold = 0; d_num_doppler_points = 0; d_doppler_step = 0; - d_grid_data = nullptr; - d_grid_doppler_wipeoffs = nullptr; d_gnss_synchro = nullptr; d_code_phase = 0; d_doppler_freq = 0; @@ -176,11 +174,7 @@ void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_ste d_num_doppler_points = floor(std::abs(2 * d_config_doppler_max) / d_doppler_step); - d_grid_data = new float *[d_num_doppler_points]; - for (int i = 0; i < d_num_doppler_points; i++) - { - d_grid_data[i] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); - } + d_grid_data = std::vector>(d_num_doppler_points, std::vector(d_fft_size)); if (d_dump) { @@ -191,25 +185,12 @@ void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_ste } -void pcps_acquisition_fine_doppler_cc::free_grid_memory() -{ - for (int i = 0; i < d_num_doppler_points; i++) - { - volk_gnsssdr_free(d_grid_data[i]); - delete[] d_grid_doppler_wipeoffs[i]; - } - delete d_grid_data; - delete d_grid_doppler_wipeoffs; -} - - pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc() { volk_gnsssdr_free(d_carrier); volk_gnsssdr_free(d_fft_codes); volk_gnsssdr_free(d_magnitude); volk_gnsssdr_free(d_10_ms_buffer); - free_grid_memory(); } @@ -265,17 +246,16 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff() // create the carrier Doppler wipeoff signals int doppler_hz; float phase_step_rad; - d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_points]; + d_grid_doppler_wipeoffs = std::vector>>(d_num_doppler_points, std::vector>(d_fft_size)); for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { doppler_hz = d_doppler_step * doppler_index - d_config_doppler_max; // doppler search steps // compute the carrier doppler wipe-off signal and store it phase_step_rad = static_cast(GPS_TWO_PI) * doppler_hz / static_cast(d_fs_in); - d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size]; float _phase[1]; _phase[0] = 0; - volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size); + volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase, d_fft_size); } } @@ -293,7 +273,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF() //--- Find the correlation peak and the carrier frequency -------------- for (int i = 0; i < d_num_doppler_points; i++) { - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i].data(), d_fft_size); if (d_grid_data[i][tmp_intex_t] > firstPeak) { firstPeak = d_grid_data[i][tmp_intex_t]; @@ -304,7 +284,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF() // Record results to file if required if (d_dump and d_channel == d_dump_channel) { - memcpy(grid_.colptr(i), d_grid_data[i], sizeof(float) * d_fft_size); + memcpy(grid_.colptr(i), d_grid_data[i].data(), sizeof(float) * d_fft_size); } } @@ -336,7 +316,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF() while (idx != excludeRangeIndex2); //--- Find the second highest correlation peak in the same freq. bin --- - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[index_doppler], d_fft_size); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[index_doppler].data(), d_fft_size); float secondPeak = d_grid_data[index_doppler][tmp_intex_t]; // 5- Compute the test statistics and compare to the threshold @@ -382,7 +362,7 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons { // doppler search steps // Perform the carrier wipe-off - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size); // 3- Perform the FFT-based convolution (parallel time search) // Compute the FFT of the carrier wiped--off incoming signal @@ -398,7 +378,7 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons // save the grid matrix delay file volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); //accumulate grid values - volk_32f_x2_add_32f(d_grid_data[doppler_index], d_grid_data[doppler_index], p_tmp_vector, d_fft_size); + volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_grid_data[doppler_index].data(), p_tmp_vector, d_fft_size); } volk_gnsssdr_free(p_tmp_vector); @@ -422,7 +402,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler() int signal_samples = prn_replicas * d_fft_size; //int fft_size_extended = nextPowerOf2(signal_samples * zero_padding_factor); int fft_size_extended = signal_samples * zero_padding_factor; - auto *fft_operator = new gr::fft::fft_complex(fft_size_extended, true); + auto fft_operator = std::make_shared(fft_size_extended, true); //zero padding the entire vector std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0)); @@ -459,9 +439,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler() //case even int counter = 0; - auto *fftFreqBins = new float[fft_size_extended]; - - std::fill_n(fftFreqBins, fft_size_extended, 0.0); + auto fftFreqBins = std::vector(fft_size_extended); for (int k = 0; k < (fft_size_extended / 2); k++) { @@ -488,10 +466,8 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler() } // free memory!! - delete fft_operator; volk_gnsssdr_free(code_replica); volk_gnsssdr_free(p_tmp_vector); - delete[] fftFreqBins; return d_fft_size; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h index 6da3e51e8..1919ec356 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h @@ -61,13 +61,13 @@ #include #include #include +#include class pcps_acquisition_fine_doppler_cc; using pcps_acquisition_fine_doppler_cc_sptr = boost::shared_ptr; -pcps_acquisition_fine_doppler_cc_sptr -pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_); +pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_); /*! * \brief This class implements a Parallel Code Phase Search Acquisition. @@ -75,63 +75,6 @@ pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_); */ class pcps_acquisition_fine_doppler_cc : public gr::block { -private: - friend pcps_acquisition_fine_doppler_cc_sptr - pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_); - pcps_acquisition_fine_doppler_cc(const Acq_Conf& conf_); - - int compute_and_accumulate_grid(gr_vector_const_void_star& input_items); - int estimate_Doppler(); - float estimate_input_power(gr_vector_const_void_star& input_items); - double compute_CAF(); - void reset_grid(); - void update_carrier_wipeoff(); - void free_grid_memory(); - bool start(); - - Acq_Conf acq_parameters; - int64_t d_fs_in; - int d_samples_per_ms; - int d_max_dwells; - int d_gnuradio_forecast_samples; - float d_threshold; - std::string d_satellite_str; - int d_config_doppler_max; - - int d_num_doppler_points; - int d_doppler_step; - unsigned int d_fft_size; - uint64_t d_sample_counter; - gr_complex* d_carrier; - gr_complex* d_fft_codes; - gr_complex* d_10_ms_buffer; - float* d_magnitude; - - float** d_grid_data; - gr_complex** d_grid_doppler_wipeoffs; - - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - unsigned int d_code_phase; - float d_doppler_freq; - float d_test_statistics; - int d_positive_acq; - - int d_state; - bool d_active; - int d_well_count; - int d_n_samples_in_buffer; - bool d_dump; - unsigned int d_channel; - std::weak_ptr d_channel_fsm; - - std::string d_dump_filename; - - arma::fmat grid_; - int64_t d_dump_number; - unsigned int d_dump_channel; - public: /*! * \brief Default destructor. @@ -221,20 +164,11 @@ public: void set_doppler_step(unsigned int doppler_step); /*! - * \brief If set to 1, ensures that acquisition starts at the - * first available sample. - * \param state - int=1 forces start of acquisition - */ - void set_state(int state); - - /*! - * \brief Parallel Code Phase Search Acquisition signal processing. + * \brief If set to 1, ensures that acquisition starts at the + * first available sample. + * \param state - int=1 forces start of acquisition */ - int general_work(int noutput_items, gr_vector_int& ninput_items, - gr_vector_const_void_star& input_items, - gr_vector_void_star& output_items); - - void forecast(int noutput_items, gr_vector_int& ninput_items_required); + void set_state(int state); /*! * \brief Obtains the next power of 2 greater or equal to the input parameter @@ -243,6 +177,64 @@ public: unsigned int nextPowerOf2(unsigned int n); void dump_results(int effective_fft_size); + + void forecast(int noutput_items, gr_vector_int& ninput_items_required); + + /*! + * \brief Parallel Code Phase Search Acquisition signal processing. + */ + int general_work(int noutput_items, gr_vector_int& ninput_items, + gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items); + +private: + friend pcps_acquisition_fine_doppler_cc_sptr + pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_); + pcps_acquisition_fine_doppler_cc(const Acq_Conf& conf_); + + int compute_and_accumulate_grid(gr_vector_const_void_star& input_items); + int estimate_Doppler(); + float estimate_input_power(gr_vector_const_void_star& input_items); + double compute_CAF(); + void reset_grid(); + void update_carrier_wipeoff(); + bool start(); + Acq_Conf acq_parameters; + int64_t d_fs_in; + int d_samples_per_ms; + int d_max_dwells; + int d_gnuradio_forecast_samples; + float d_threshold; + std::string d_satellite_str; + int d_config_doppler_max; + int d_num_doppler_points; + int d_doppler_step; + unsigned int d_fft_size; + uint64_t d_sample_counter; + gr_complex* d_carrier; + gr_complex* d_fft_codes; + gr_complex* d_10_ms_buffer; + float* d_magnitude; + std::vector> d_grid_data; + std::vector>> d_grid_doppler_wipeoffs; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + unsigned int d_code_phase; + float d_doppler_freq; + float d_test_statistics; + int d_positive_acq; + int d_state; + bool d_active; + int d_well_count; + int d_n_samples_in_buffer; + bool d_dump; + unsigned int d_channel; + std::weak_ptr d_channel_fsm; + std::string d_dump_filename; + arma::fmat grid_; + int64_t d_dump_number; + unsigned int d_dump_channel; }; #endif /* pcps_acquisition_fine_doppler_cc*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 99de2d8ed..2d9929063 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -34,7 +34,6 @@ #include "pcps_acquisition_fpga.h" #include "gnss_synchro.h" -//#include #include #include // for ceil #include // for operator<< @@ -87,6 +86,7 @@ pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) pcps_acquisition_fpga::~pcps_acquisition_fpga() = default; + void pcps_acquisition_fpga::set_local_code() { acquisition_fpga->set_local_code(d_gnss_synchro->PRN); @@ -174,6 +174,7 @@ void pcps_acquisition_fpga::send_negative_acquisition() } } + void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_min) { uint32_t indext = 0U; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h index 307dcba45..81f75766d 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h @@ -62,7 +62,7 @@ typedef struct int32_t code_length; uint32_t select_queue_Fpga; std::string device_name; - uint32_t* all_fft_codes; // pointer to memory that contains all the code ffts + uint32_t* all_fft_codes; // pointer to memory that contains all the code ffts //float downsampling_factor; uint32_t downsampling_factor; uint32_t total_block_exp; @@ -78,8 +78,7 @@ class pcps_acquisition_fpga; using pcps_acquisition_fpga_sptr = boost::shared_ptr; -pcps_acquisition_fpga_sptr -pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); +pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); /*! * \brief This class implements a Parallel Code Phase Search Acquisition that uses the FPGA. @@ -89,49 +88,6 @@ pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); */ class pcps_acquisition_fpga { -private: - friend pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); - - pcps_acquisition_fpga(pcpsconf_fpga_t conf_); - - void send_negative_acquisition(); - - void send_positive_acquisition(); - - float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); - - void acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_max); - - pcpsconf_fpga_t acq_parameters; - bool d_active; - float d_threshold; - float d_mag; - float d_input_power; - uint32_t d_doppler_index; - float d_test_statistics; - int32_t d_state; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - uint32_t d_doppler_step; - uint32_t d_doppler_max; - uint32_t d_fft_size; - uint32_t d_num_doppler_bins; - uint64_t d_sample_counter; - Gnss_Synchro* d_gnss_synchro; - std::shared_ptr acquisition_fpga; - - //float d_downsampling_factor; - uint32_t d_downsampling_factor; - uint32_t d_select_queue_Fpga; - - uint32_t d_total_block_exp; - - bool d_make_2_steps; - uint32_t d_num_doppler_bins_step2; - float d_doppler_step2; - float d_doppler_center_step_two; - uint32_t d_max_num_acqs; - public: ~pcps_acquisition_fpga(); @@ -229,6 +185,39 @@ public: * \brief This funciton triggers a HW reset of the FPGA PL. */ void reset_acquisition(void); + +private: + friend pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); + pcps_acquisition_fpga(pcpsconf_fpga_t conf_); + bool d_active; + bool d_make_2_steps; + uint32_t d_doppler_index; + uint32_t d_channel; + uint32_t d_doppler_step; + uint32_t d_doppler_max; + uint32_t d_fft_size; + uint32_t d_num_doppler_bins; + uint32_t d_downsampling_factor; + uint32_t d_select_queue_Fpga; + uint32_t d_total_block_exp; + uint32_t d_num_doppler_bins_step2; + uint32_t d_max_num_acqs; + int32_t d_state; + uint64_t d_sample_counter; + float d_threshold; + float d_mag; + float d_input_power; + float d_test_statistics; + float d_doppler_step2; + float d_doppler_center_step_two; + pcpsconf_fpga_t acq_parameters; + Gnss_Synchro* d_gnss_synchro; + std::shared_ptr acquisition_fpga; + std::weak_ptr d_channel_fsm; + void send_negative_acquisition(); + void send_positive_acquisition(); + void acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_max); + float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); }; #endif /* GNSS_SDR_PCPS_ACQUISITION_FPGA_H_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index 20221f51b..a713357b2 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -97,8 +97,6 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc( d_doppler_min = 0; d_num_doppler_points = 0; d_doppler_step = 0; - d_grid_data = nullptr; - d_grid_doppler_wipeoffs = nullptr; d_gnss_synchro = nullptr; d_code_phase = 0; d_doppler_freq = 0; @@ -114,17 +112,6 @@ void pcps_assisted_acquisition_cc::set_doppler_step(uint32_t doppler_step) } -void pcps_assisted_acquisition_cc::free_grid_memory() -{ - for (int32_t i = 0; i < d_num_doppler_points; i++) - { - delete[] d_grid_data[i]; - delete[] d_grid_doppler_wipeoffs[i]; - } - delete d_grid_data; -} - - pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc() { volk_gnsssdr_free(d_carrier); @@ -234,26 +221,21 @@ void pcps_assisted_acquisition_cc::redefine_grid() // Create the search grid array d_num_doppler_points = floor(std::abs(d_doppler_max - d_doppler_min) / d_doppler_step); - d_grid_data = new float *[d_num_doppler_points]; - for (int32_t i = 0; i < d_num_doppler_points; i++) - { - d_grid_data[i] = new float[d_fft_size]; - } + d_grid_data = std::vector>(d_num_doppler_points, std::vector(d_fft_size)); // create the carrier Doppler wipeoff signals int32_t doppler_hz; float phase_step_rad; - d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_points]; + d_grid_doppler_wipeoffs = std::vector>>(d_num_doppler_points, std::vector>(d_fft_size)); for (int32_t doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { doppler_hz = d_doppler_min + d_doppler_step * doppler_index; // doppler search steps // compute the carrier doppler wipe-off signal and store it phase_step_rad = static_cast(GPS_TWO_PI) * doppler_hz / static_cast(d_fs_in); - d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size]; float _phase[1]; _phase[0] = 0; - volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size); + volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase, d_fft_size); } } @@ -268,7 +250,7 @@ double pcps_assisted_acquisition_cc::search_maximum() for (int32_t i = 0; i < d_num_doppler_points; i++) { - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i].data(), d_fft_size); if (d_grid_data[i][tmp_intex_t] > magt) { magt = d_grid_data[i][index_time]; @@ -300,7 +282,7 @@ double pcps_assisted_acquisition_cc::search_maximum() << "_" << d_gnss_synchro->Signal << "_sat_" << d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat"; d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); - d_dump_file.write(reinterpret_cast(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin? + d_dump_file.write(reinterpret_cast(d_grid_data[index_doppler].data()), n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.close(); } @@ -343,7 +325,7 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons { // doppler search steps // Perform the carrier wipe-off - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size); // 3- Perform the FFT-based convolution (parallel time search) // Compute the FFT of the carrier wiped--off incoming signal d_fft_if->execute(); @@ -357,8 +339,8 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons // save the grid matrix delay file volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); - const float *old_vector = d_grid_data[doppler_index]; - volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size); + const float *old_vector = d_grid_data[doppler_index].data(); + volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, p_tmp_vector, d_fft_size); } volk_gnsssdr_free(p_tmp_vector); return d_fft_size; @@ -439,7 +421,6 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items, consume_each(ninput_items[0]); break; case 4: // RedefineGrid - free_grid_memory(); redefine_grid(); reset_grid(); d_sample_counter += static_cast(ninput_items[0]); // sample counter @@ -458,7 +439,6 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items, d_active = false; // Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL this->message_port_pub(pmt::mp("events"), pmt::from_long(1)); - free_grid_memory(); // consume samples to not block the GNU Radio flowgraph d_sample_counter += static_cast(ninput_items[0]); // sample counter consume_each(ninput_items[0]); @@ -476,7 +456,6 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items, d_active = false; // Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL this->message_port_pub(pmt::mp("events"), pmt::from_long(2)); - free_grid_memory(); // consume samples to not block the GNU Radio flowgraph d_sample_counter += static_cast(ninput_items[0]); // sample counter consume_each(ninput_items[0]); diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h index 208725b1f..3c57dd80b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h @@ -57,13 +57,13 @@ #include #include #include +#include class pcps_assisted_acquisition_cc; using pcps_assisted_acquisition_cc_sptr = boost::shared_ptr; -pcps_assisted_acquisition_cc_sptr -pcps_make_assisted_acquisition_cc( +pcps_assisted_acquisition_cc_sptr pcps_make_assisted_acquisition_cc( int32_t max_dwells, uint32_t sampled_ms, int32_t doppler_max, @@ -80,69 +80,6 @@ pcps_make_assisted_acquisition_cc( */ class pcps_assisted_acquisition_cc : public gr::block { -private: - friend pcps_assisted_acquisition_cc_sptr - pcps_make_assisted_acquisition_cc(int32_t max_dwells, uint32_t sampled_ms, - int32_t doppler_max, int32_t doppler_min, int64_t fs_in, - int32_t samples_per_ms, bool dump, - std::string dump_filename); - - pcps_assisted_acquisition_cc(int32_t max_dwells, uint32_t sampled_ms, - int32_t doppler_max, int32_t doppler_min, int64_t fs_in, - int32_t samples_per_ms, bool dump, - std::string dump_filename); - - void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, - int32_t doppler_offset); - - int32_t compute_and_accumulate_grid(gr_vector_const_void_star& input_items); - float estimate_input_power(gr_vector_const_void_star& input_items); - double search_maximum(); - void get_assistance(); - void reset_grid(); - void redefine_grid(); - void free_grid_memory(); - - int64_t d_fs_in; - int32_t d_samples_per_ms; - int32_t d_max_dwells; - uint32_t d_doppler_resolution; - int32_t d_gnuradio_forecast_samples; - float d_threshold; - std::string d_satellite_str; - int32_t d_doppler_max; - int32_t d_doppler_min; - int32_t d_config_doppler_max; - int32_t d_config_doppler_min; - - int32_t d_num_doppler_points; - int32_t d_doppler_step; - uint32_t d_sampled_ms; - uint32_t d_fft_size; - uint64_t d_sample_counter; - gr_complex* d_carrier; - gr_complex* d_fft_codes; - - float** d_grid_data; - gr_complex** d_grid_doppler_wipeoffs; - - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - uint32_t d_code_phase; - float d_doppler_freq; - float d_input_power; - float d_test_statistics; - std::ofstream d_dump_file; - int32_t d_state; - bool d_active; - bool d_disable_assist; - int32_t d_well_count; - bool d_dump; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - std::string d_dump_filename; - public: /*! * \brief Default destructor. @@ -238,6 +175,68 @@ public: gr_vector_void_star& output_items); void forecast(int noutput_items, gr_vector_int& ninput_items_required); + +private: + friend pcps_assisted_acquisition_cc_sptr + pcps_make_assisted_acquisition_cc(int32_t max_dwells, uint32_t sampled_ms, + int32_t doppler_max, int32_t doppler_min, int64_t fs_in, + int32_t samples_per_ms, bool dump, + std::string dump_filename); + + pcps_assisted_acquisition_cc(int32_t max_dwells, uint32_t sampled_ms, + int32_t doppler_max, int32_t doppler_min, int64_t fs_in, + int32_t samples_per_ms, bool dump, + std::string dump_filename); + + void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, + int32_t doppler_offset); + + int32_t compute_and_accumulate_grid(gr_vector_const_void_star& input_items); + float estimate_input_power(gr_vector_const_void_star& input_items); + double search_maximum(); + void get_assistance(); + void reset_grid(); + void redefine_grid(); + + int64_t d_fs_in; + int32_t d_samples_per_ms; + int32_t d_max_dwells; + uint32_t d_doppler_resolution; + int32_t d_gnuradio_forecast_samples; + float d_threshold; + std::string d_satellite_str; + int32_t d_doppler_max; + int32_t d_doppler_min; + int32_t d_config_doppler_max; + int32_t d_config_doppler_min; + + int32_t d_num_doppler_points; + int32_t d_doppler_step; + uint32_t d_sampled_ms; + uint32_t d_fft_size; + uint64_t d_sample_counter; + gr_complex* d_carrier; + gr_complex* d_fft_codes; + + std::vector> d_grid_data; + std::vector>> d_grid_doppler_wipeoffs; + + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + uint32_t d_code_phase; + float d_doppler_freq; + float d_input_power; + float d_test_statistics; + std::ofstream d_dump_file; + int32_t d_state; + bool d_active; + bool d_disable_assist; + int32_t d_well_count; + bool d_dump; + uint32_t d_channel; + std::weak_ptr d_channel_fsm; + std::string d_dump_filename; }; #endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h index 7d5bc3b0e..9fed31338 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h @@ -52,8 +52,7 @@ class pcps_cccwsr_acquisition_cc; using pcps_cccwsr_acquisition_cc_sptr = boost::shared_ptr; -pcps_cccwsr_acquisition_cc_sptr -pcps_cccwsr_make_acquisition_cc( +pcps_cccwsr_acquisition_cc_sptr pcps_cccwsr_make_acquisition_cc( uint32_t sampled_ms, uint32_t max_dwells, uint32_t doppler_max, @@ -69,59 +68,6 @@ pcps_cccwsr_make_acquisition_cc( */ class pcps_cccwsr_acquisition_cc : public gr::block { -private: - friend pcps_cccwsr_acquisition_cc_sptr - pcps_cccwsr_make_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, - uint32_t doppler_max, int64_t fs_in, - int32_t samples_per_ms, int32_t samples_per_code, - bool dump, std::string dump_filename); - - pcps_cccwsr_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, - uint32_t doppler_max, int64_t fs_in, - int32_t samples_per_ms, int32_t samples_per_code, - bool dump, std::string dump_filename); - - void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, - int32_t doppler_offset); - - int64_t d_fs_in; - int32_t d_samples_per_ms; - int32_t d_samples_per_code; - uint32_t d_doppler_resolution; - float d_threshold; - std::string d_satellite_str; - uint32_t d_doppler_max; - uint32_t d_doppler_step; - uint32_t d_sampled_ms; - uint32_t d_max_dwells; - uint32_t d_well_count; - uint32_t d_fft_size; - uint64_t d_sample_counter; - gr_complex** d_grid_doppler_wipeoffs; - uint32_t d_num_doppler_bins; - gr_complex* d_fft_code_data; - gr_complex* d_fft_code_pilot; - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - uint32_t d_code_phase; - float d_doppler_freq; - float d_mag; - float* d_magnitude; - gr_complex* d_data_correlation; - gr_complex* d_pilot_correlation; - gr_complex* d_correlation_plus; - gr_complex* d_correlation_minus; - float d_input_power; - float d_test_statistics; - std::ofstream d_dump_file; - bool d_active; - int32_t d_state; - bool d_dump; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - std::string d_dump_filename; - public: /*! * \brief Default destructor. @@ -226,6 +172,59 @@ public: int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + +private: + friend pcps_cccwsr_acquisition_cc_sptr + pcps_cccwsr_make_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, + uint32_t doppler_max, int64_t fs_in, + int32_t samples_per_ms, int32_t samples_per_code, + bool dump, std::string dump_filename); + + pcps_cccwsr_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, + uint32_t doppler_max, int64_t fs_in, + int32_t samples_per_ms, int32_t samples_per_code, + bool dump, std::string dump_filename); + + void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, + int32_t doppler_offset); + + int64_t d_fs_in; + int32_t d_samples_per_ms; + int32_t d_samples_per_code; + uint32_t d_doppler_resolution; + float d_threshold; + std::string d_satellite_str; + uint32_t d_doppler_max; + uint32_t d_doppler_step; + uint32_t d_sampled_ms; + uint32_t d_max_dwells; + uint32_t d_well_count; + uint32_t d_fft_size; + uint64_t d_sample_counter; + gr_complex** d_grid_doppler_wipeoffs; + uint32_t d_num_doppler_bins; + gr_complex* d_fft_code_data; + gr_complex* d_fft_code_pilot; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + uint32_t d_code_phase; + float d_doppler_freq; + float d_mag; + float* d_magnitude; + gr_complex* d_data_correlation; + gr_complex* d_pilot_correlation; + gr_complex* d_correlation_plus; + gr_complex* d_correlation_minus; + float d_input_power; + float d_test_statistics; + std::ofstream d_dump_file; + bool d_active; + int32_t d_state; + bool d_dump; + uint32_t d_channel; + std::weak_ptr d_channel_fsm; + std::string d_dump_filename; }; #endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h index bab96e123..5eec41228 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h @@ -54,11 +54,11 @@ #define CL_SILENCE_DEPRECATION #include "channel_fsm.h" #include "gnss_synchro.h" -#include "opencl/cl.hpp" #include "opencl/fft_internal.h" #include #include #include +#include "opencl/cl.hpp" #include #include #include @@ -68,10 +68,13 @@ class pcps_opencl_acquisition_cc; typedef boost::shared_ptr pcps_opencl_acquisition_cc_sptr; -pcps_opencl_acquisition_cc_sptr -pcps_make_opencl_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, - uint32_t doppler_max, int64_t fs_in, - int samples_per_ms, int samples_per_code, +pcps_opencl_acquisition_cc_sptr pcps_make_opencl_acquisition_cc( + uint32_t sampled_ms, + uint32_t max_dwells, + uint32_t doppler_max, + int64_t fs_in, + int samples_per_ms, + int samples_per_code, bool bit_transition_flag, bool dump, std::string dump_filename); @@ -84,6 +87,124 @@ pcps_make_opencl_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, */ class pcps_opencl_acquisition_cc : public gr::block { +public: + /*! + * \brief Default destructor. + */ + ~pcps_opencl_acquisition_cc(); + + /*! + * \brief Set acquisition/tracking common Gnss_Synchro object pointer + * to exchange synchronization data between acquisition and tracking blocks. + * \param p_gnss_synchro Satellite information shared by the processing blocks. + */ + inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) + { + d_gnss_synchro = p_gnss_synchro; + } + + /*! + * \brief Returns the maximum peak of grid search. + */ + inline uint32_t mag() const + { + return d_mag; + } + + /*! + * \brief Initializes acquisition algorithm. + */ + void init(); + + /*! + * \brief Sets local code for PCPS acquisition algorithm. + * \param code - Pointer to the PRN code. + */ + void set_local_code(std::complex* code); + + /*! + * \brief Starts acquisition algorithm, turning from standby mode to + * active mode + * \param active - bool that activates/deactivates the block. + */ + inline void set_active(bool active) + { + d_active = active; + } + + /*! + * \brief If set to 1, ensures that acquisition starts at the + * first available sample. + * \param state - int=1 forces start of acquisition + */ + void set_state(int state); + + /*! + * \brief Set acquisition channel unique ID + * \param channel - receiver channel. + */ + inline void set_channel(uint32_t channel) + { + d_channel = channel; + } + + /*! + * \brief Set channel fsm associated to this acquisition instance + */ + inline void set_channel_fsm(std::weak_ptr channel_fsm) + { + d_channel_fsm = channel_fsm; + } + + /*! + * \brief Set statistics threshold of PCPS algorithm. + * \param threshold - Threshold for signal detection (check \ref Navitec2012, + * Algorithm 1, for a definition of this threshold). + */ + inline void set_threshold(float threshold) + { + d_threshold = threshold; + } + + /*! + * \brief Set maximum Doppler grid search + * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. + */ + inline void set_doppler_max(uint32_t doppler_max) + { + d_doppler_max = doppler_max; + } + + /*! + * \brief Set Doppler steps for the grid search + * \param doppler_step - Frequency bin of the search grid [Hz]. + */ + inline void set_doppler_step(uint32_t doppler_step) + { + d_doppler_step = doppler_step; + } + + inline bool opencl_ready() const + { + bool ready = false; + if (d_opencl == 0) + { + ready = true; + } + return ready; + } + + void acquisition_core_volk(); + + void acquisition_core_opencl(); + + /*! + * \brief Parallel Code Phase Search Acquisition signal processing. + */ + int general_work(int noutput_items, gr_vector_int& ninput_items, + gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items); + private: friend pcps_opencl_acquisition_cc_sptr pcps_make_opencl_acquisition_cc(uint32_t sampled_ms, uint32_t max_dwells, @@ -144,6 +265,8 @@ private: gr_complex** d_in_buffer; std::vector d_sample_counter_buffer; uint32_t d_in_dwell_count; + std::weak_ptr d_channel_fsm; + int d_opencl; cl::Platform d_cl_platform; cl::Device d_cl_device; @@ -158,125 +281,6 @@ private: cl::CommandQueue* d_cl_queue; clFFT_Plan d_cl_fft_plan; cl_int d_cl_fft_batch_size; - std::weak_ptr d_channel_fsm; - int d_opencl; - -public: - /*! - * \brief Default destructor. - */ - ~pcps_opencl_acquisition_cc(); - - /*! - * \brief Set acquisition/tracking common Gnss_Synchro object pointer - * to exchange synchronization data between acquisition and tracking blocks. - * \param p_gnss_synchro Satellite information shared by the processing blocks. - */ - inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) - { - d_gnss_synchro = p_gnss_synchro; - } - - /*! - * \brief Returns the maximum peak of grid search. - */ - inline uint32_t mag() const - { - return d_mag; - } - - /*! - * \brief Initializes acquisition algorithm. - */ - void init(); - - /*! - * \brief Sets local code for PCPS acquisition algorithm. - * \param code - Pointer to the PRN code. - */ - void set_local_code(std::complex* code); - - /*! - * \brief Starts acquisition algorithm, turning from standby mode to - * active mode - * \param active - bool that activates/deactivates the block. - */ - inline void set_active(bool active) - { - d_active = active; - } - - /*! - * \brief If set to 1, ensures that acquisition starts at the - * first available sample. - * \param state - int=1 forces start of acquisition - */ - void set_state(int state); - - /*! - * \brief Set acquisition channel unique ID - * \param channel - receiver channel. - */ - inline void set_channel(uint32_t channel) - { - d_channel = channel; - } - - /*! - * \brief Set channel fsm associated to this acquisition instance - */ - inline void set_channel_fsm(std::weak_ptr channel_fsm) - { - d_channel_fsm = channel_fsm; - } - /*! - * \brief Set statistics threshold of PCPS algorithm. - * \param threshold - Threshold for signal detection (check \ref Navitec2012, - * Algorithm 1, for a definition of this threshold). - */ - inline void set_threshold(float threshold) - { - d_threshold = threshold; - } - - /*! - * \brief Set maximum Doppler grid search - * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. - */ - inline void set_doppler_max(uint32_t doppler_max) - { - d_doppler_max = doppler_max; - } - - /*! - * \brief Set Doppler steps for the grid search - * \param doppler_step - Frequency bin of the search grid [Hz]. - */ - inline void set_doppler_step(uint32_t doppler_step) - { - d_doppler_step = doppler_step; - } - - inline bool opencl_ready() const - { - bool ready = false; - if (d_opencl == 0) - { - ready = true; - } - return ready; - } - - /*! - * \brief Parallel Code Phase Search Acquisition signal processing. - */ - int general_work(int noutput_items, gr_vector_int& ninput_items, - gr_vector_const_void_star& input_items, - gr_vector_void_star& output_items); - - void acquisition_core_volk(); - - void acquisition_core_opencl(); }; #endif diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h index 17e80aa95..2f0bdda3a 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h @@ -67,8 +67,7 @@ class pcps_quicksync_acquisition_cc; using pcps_quicksync_acquisition_cc_sptr = boost::shared_ptr; -pcps_quicksync_acquisition_cc_sptr -pcps_quicksync_make_acquisition_cc( +pcps_quicksync_acquisition_cc_sptr pcps_quicksync_make_acquisition_cc( uint32_t folding_factor, uint32_t sampled_ms, uint32_t max_dwells, @@ -89,71 +88,6 @@ pcps_quicksync_make_acquisition_cc( */ class pcps_quicksync_acquisition_cc : public gr::block { -private: - friend pcps_quicksync_acquisition_cc_sptr - pcps_quicksync_make_acquisition_cc(uint32_t folding_factor, - uint32_t sampled_ms, uint32_t max_dwells, - uint32_t doppler_max, int64_t fs_in, - int32_t samples_per_ms, int32_t samples_per_code, - bool bit_transition_flag, - bool dump, - std::string dump_filename); - - pcps_quicksync_acquisition_cc(uint32_t folding_factor, - uint32_t sampled_ms, uint32_t max_dwells, - uint32_t doppler_max, int64_t fs_in, - int32_t samples_per_ms, int32_t samples_per_code, - bool bit_transition_flag, - bool dump, - std::string dump_filename); - - void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, - int32_t doppler_offset); - - gr_complex* d_code; - uint32_t d_folding_factor; // also referred in the paper as 'p' - float* d_corr_acumulator; - uint32_t* d_possible_delay; - float* d_corr_output_f; - float* d_magnitude_folded; - gr_complex* d_signal_folded; - gr_complex* d_code_folded; - float d_noise_floor_power; - - int64_t d_fs_in; - int32_t d_samples_per_ms; - int32_t d_samples_per_code; - uint32_t d_doppler_resolution; - float d_threshold; - std::string d_satellite_str; - uint32_t d_doppler_max; - uint32_t d_doppler_step; - uint32_t d_sampled_ms; - uint32_t d_max_dwells; - uint32_t d_well_count; - uint32_t d_fft_size; - uint64_t d_sample_counter; - gr_complex** d_grid_doppler_wipeoffs; - uint32_t d_num_doppler_bins; - gr_complex* d_fft_codes; - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - uint32_t d_code_phase; - float d_doppler_freq; - float d_mag; - float* d_magnitude; - float d_input_power; - float d_test_statistics; - bool d_bit_transition_flag; - std::ofstream d_dump_file; - bool d_active; - int32_t d_state; - bool d_dump; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - std::string d_dump_filename; - public: /*! * \brief Default destructor. @@ -257,6 +191,70 @@ public: int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + +private: + friend pcps_quicksync_acquisition_cc_sptr + pcps_quicksync_make_acquisition_cc(uint32_t folding_factor, + uint32_t sampled_ms, uint32_t max_dwells, + uint32_t doppler_max, int64_t fs_in, + int32_t samples_per_ms, int32_t samples_per_code, + bool bit_transition_flag, + bool dump, + std::string dump_filename); + + pcps_quicksync_acquisition_cc(uint32_t folding_factor, + uint32_t sampled_ms, uint32_t max_dwells, + uint32_t doppler_max, int64_t fs_in, + int32_t samples_per_ms, int32_t samples_per_code, + bool bit_transition_flag, + bool dump, + std::string dump_filename); + + void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, + int32_t doppler_offset); + + gr_complex* d_code; + uint32_t d_folding_factor; // also referred in the paper as 'p' + float* d_corr_acumulator; + uint32_t* d_possible_delay; + float* d_corr_output_f; + float* d_magnitude_folded; + gr_complex* d_signal_folded; + gr_complex* d_code_folded; + float d_noise_floor_power; + int64_t d_fs_in; + int32_t d_samples_per_ms; + int32_t d_samples_per_code; + uint32_t d_doppler_resolution; + float d_threshold; + std::string d_satellite_str; + uint32_t d_doppler_max; + uint32_t d_doppler_step; + uint32_t d_sampled_ms; + uint32_t d_max_dwells; + uint32_t d_well_count; + uint32_t d_fft_size; + uint64_t d_sample_counter; + gr_complex** d_grid_doppler_wipeoffs; + uint32_t d_num_doppler_bins; + gr_complex* d_fft_codes; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + uint32_t d_code_phase; + float d_doppler_freq; + float d_mag; + float* d_magnitude; + float d_input_power; + float d_test_statistics; + bool d_bit_transition_flag; + std::ofstream d_dump_file; + bool d_active; + int32_t d_state; + bool d_dump; + uint32_t d_channel; + std::weak_ptr d_channel_fsm; + std::string d_dump_filename; }; -#endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/ +#endif /* GNSS_SDR_PCPS_QUICKSYNC_ACQUISITION_CC_H_ */ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h index 5c8f4c81c..c4a9e77f0 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h @@ -65,8 +65,7 @@ class pcps_tong_acquisition_cc; using pcps_tong_acquisition_cc_sptr = boost::shared_ptr; -pcps_tong_acquisition_cc_sptr -pcps_tong_make_acquisition_cc( +pcps_tong_acquisition_cc_sptr pcps_tong_make_acquisition_cc( uint32_t sampled_ms, uint32_t doppler_max, int64_t fs_in, @@ -84,60 +83,6 @@ pcps_tong_make_acquisition_cc( */ class pcps_tong_acquisition_cc : public gr::block { -private: - friend pcps_tong_acquisition_cc_sptr - pcps_tong_make_acquisition_cc(uint32_t sampled_ms, uint32_t doppler_max, - int64_t fs_in, int32_t samples_per_ms, - int32_t samples_per_code, uint32_t tong_init_val, - uint32_t tong_max_val, uint32_t tong_max_dwells, - bool dump, std::string dump_filename); - - pcps_tong_acquisition_cc(uint32_t sampled_ms, uint32_t doppler_max, - int64_t fs_in, int32_t samples_per_ms, - int32_t samples_per_code, uint32_t tong_init_val, - uint32_t tong_max_val, uint32_t tong_max_dwells, - bool dump, std::string dump_filename); - - void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, - int32_t doppler_offset); - - int64_t d_fs_in; - int32_t d_samples_per_ms; - int32_t d_samples_per_code; - uint32_t d_doppler_resolution; - float d_threshold; - std::string d_satellite_str; - uint32_t d_doppler_max; - uint32_t d_doppler_step; - uint32_t d_sampled_ms; - uint32_t d_dwell_count; - uint32_t d_tong_count; - uint32_t d_tong_init_val; - uint32_t d_tong_max_val; - uint32_t d_tong_max_dwells; - uint32_t d_fft_size; - uint64_t d_sample_counter; - gr_complex** d_grid_doppler_wipeoffs; - uint32_t d_num_doppler_bins; - gr_complex* d_fft_codes; - float** d_grid_data; - std::shared_ptr d_fft_if; - std::shared_ptr d_ifft; - Gnss_Synchro* d_gnss_synchro; - uint32_t d_code_phase; - float d_doppler_freq; - float d_mag; - float* d_magnitude; - float d_input_power; - float d_test_statistics; - std::ofstream d_dump_file; - bool d_active; - int32_t d_state; - bool d_dump; - uint32_t d_channel; - std::weak_ptr d_channel_fsm; - std::string d_dump_filename; - public: /*! * \brief Default destructor. @@ -241,6 +186,60 @@ public: int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + +private: + friend pcps_tong_acquisition_cc_sptr + pcps_tong_make_acquisition_cc(uint32_t sampled_ms, uint32_t doppler_max, + int64_t fs_in, int32_t samples_per_ms, + int32_t samples_per_code, uint32_t tong_init_val, + uint32_t tong_max_val, uint32_t tong_max_dwells, + bool dump, std::string dump_filename); + + pcps_tong_acquisition_cc(uint32_t sampled_ms, uint32_t doppler_max, + int64_t fs_in, int32_t samples_per_ms, + int32_t samples_per_code, uint32_t tong_init_val, + uint32_t tong_max_val, uint32_t tong_max_dwells, + bool dump, std::string dump_filename); + + void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, + int32_t doppler_offset); + + int64_t d_fs_in; + int32_t d_samples_per_ms; + int32_t d_samples_per_code; + uint32_t d_doppler_resolution; + float d_threshold; + std::string d_satellite_str; + uint32_t d_doppler_max; + uint32_t d_doppler_step; + uint32_t d_sampled_ms; + uint32_t d_dwell_count; + uint32_t d_tong_count; + uint32_t d_tong_init_val; + uint32_t d_tong_max_val; + uint32_t d_tong_max_dwells; + uint32_t d_fft_size; + uint64_t d_sample_counter; + gr_complex** d_grid_doppler_wipeoffs; + uint32_t d_num_doppler_bins; + gr_complex* d_fft_codes; + float** d_grid_data; + std::shared_ptr d_fft_if; + std::shared_ptr d_ifft; + Gnss_Synchro* d_gnss_synchro; + uint32_t d_code_phase; + float d_doppler_freq; + float d_mag; + float* d_magnitude; + float d_input_power; + float d_test_statistics; + std::ofstream d_dump_file; + bool d_active; + int32_t d_state; + bool d_dump; + uint32_t d_channel; + std::weak_ptr d_channel_fsm; + std::string d_dump_filename; }; #endif /* GNSS_SDR_PCPS_TONG_ACQUISITION_CC_H_ */ From 018cde89536b870011e87fa787913eb8d60ec46e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 1 Jul 2019 23:44:42 +0200 Subject: [PATCH 31/64] Write public interface first, then private members in class definitions --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 102 +++++++------- src/algorithms/PVT/libs/geojson_printer.h | 12 +- src/algorithms/PVT/libs/gpx_printer.h | 14 +- src/algorithms/PVT/libs/hybrid_ls_pvt.h | 16 +-- src/algorithms/PVT/libs/kml_printer.h | 14 +- src/algorithms/PVT/libs/ls_pvt.h | 12 +- src/algorithms/PVT/libs/pvt_solution.h | 125 +++++++++--------- src/algorithms/PVT/libs/rtklib_solver.h | 23 ++-- .../channel/libs/channel_msg_receiver_cc.h | 6 +- .../interleaved_byte_to_complex_byte.h | 9 +- .../interleaved_byte_to_complex_short.h | 11 +- .../interleaved_short_to_complex_short.h | 11 +- .../input_filter/gnuradio_blocks/beamformer.h | 10 +- .../input_filter/gnuradio_blocks/notch_cc.h | 25 ++-- .../gnuradio_blocks/notch_lite_cc.h | 25 ++-- .../gnuradio_blocks/pulse_blanking_cc.h | 22 +-- .../direct_resampler_conditioner_cb.h | 34 ++--- .../direct_resampler_conditioner_cc.h | 33 +++-- .../direct_resampler_conditioner_cs.h | 34 ++--- .../gnuradio_blocks/signal_generator_c.h | 81 +++++++----- .../gr_complex_ip_packet_source.h | 75 +++++------ .../gnuradio_blocks/labsat23_source.h | 31 +++-- .../gnuradio_blocks/unpack_2bit_samples.h | 34 ++--- .../unpack_byte_2bit_cpx_samples.h | 6 +- .../unpack_byte_2bit_samples.h | 7 +- .../unpack_byte_4bit_samples.h | 6 +- .../unpack_intspir_1bit_samples.h | 7 +- .../signal_source/libs/gnss_sdr_valve.h | 35 +++-- .../signal_source/libs/rtl_tcp_dongle_info.h | 10 +- .../tracking/libs/tracking_2nd_DLL_filter.h | 18 +-- .../tracking/libs/tracking_2nd_PLL_filter.h | 29 ++-- .../tracking/libs/tracking_FLL_PLL_filter.h | 14 +- .../tracking/libs/tracking_loop_filter.h | 42 +++--- src/core/libs/gnss_sdr_fpga_sample_counter.h | 14 +- src/core/libs/gnss_sdr_sample_counter.h | 23 ++-- src/core/libs/gnss_sdr_supl_client.h | 29 ++-- src/core/libs/gnss_sdr_time_counter.h | 12 +- src/core/monitor/gnss_synchro_monitor.h | 12 +- src/core/receiver/concurrent_map.h | 8 +- src/core/receiver/concurrent_queue.h | 10 +- .../system_parameters/beidou_dnav_ephemeris.h | 67 +++++----- .../beidou_dnav_navigation_message.h | 29 ++-- .../galileo_navigation_message.h | 16 +-- .../glonass_gnav_ephemeris.h | 90 ++++++------- .../glonass_gnav_navigation_message.h | 22 +-- .../glonass_gnav_utc_model.h | 22 +-- src/core/system_parameters/gnss_signal.h | 8 +- .../system_parameters/gps_cnav_ephemeris.h | 49 +++---- .../gps_cnav_navigation_message.h | 26 ++-- src/core/system_parameters/gps_ephemeris.h | 64 ++++----- .../gps_navigation_message.h | 20 +-- src/core/system_parameters/sbas_ephemeris.h | 22 ++- 52 files changed, 757 insertions(+), 719 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index ade48e8b7..595cd2624 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -77,6 +77,57 @@ rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels, */ class rtklib_pvt_gs : public gr::sync_block { +public: + ~rtklib_pvt_gs(); //!< Default destructor + + /*! + * \brief Get latest set of GPS ephemeris from PVT block + */ + std::map get_gps_ephemeris_map() const; + + /*! + * \brief Get latest set of GPS almanac from PVT block + */ + std::map get_gps_almanac_map() const; + + /*! + * \brief Get latest set of Galileo ephemeris from PVT block + */ + std::map get_galileo_ephemeris_map() const; + + /*! + * \brief Get latest set of Galileo almanac from PVT block + */ + std::map get_galileo_almanac_map() const; + + /*! + * \brief Get latest set of BeiDou DNAV ephemeris from PVT block + */ + std::map get_beidou_dnav_ephemeris_map() const; + + /*! + * \brief Get latest set of BeiDou DNAV almanac from PVT block + */ + std::map get_beidou_dnav_almanac_map() const; + + /*! + * \brief Clear all ephemeris information and the almanacs for GPS and Galileo + */ + void clear_ephemeris(); + + /*! + * \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available + */ + bool get_latest_PVT(double* longitude_deg, + double* latitude_deg, + double* height_m, + double* ground_speed_kmh, + double* course_over_ground_deg, + time_t* UTC_time) const; + + int work(int noutput_items, gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items); //!< PVT Signal Processing + private: friend rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, @@ -169,57 +220,6 @@ private: bool d_show_local_time_zone; std::string d_local_time_str; boost::posix_time::time_duration d_utc_diff_time; - -public: - ~rtklib_pvt_gs(); //!< Default destructor - - /*! - * \brief Get latest set of GPS ephemeris from PVT block - */ - std::map get_gps_ephemeris_map() const; - - /*! - * \brief Get latest set of GPS almanac from PVT block - */ - std::map get_gps_almanac_map() const; - - /*! - * \brief Get latest set of Galileo ephemeris from PVT block - */ - std::map get_galileo_ephemeris_map() const; - - /*! - * \brief Get latest set of Galileo almanac from PVT block - */ - std::map get_galileo_almanac_map() const; - - /*! - * \brief Get latest set of BeiDou DNAV ephemeris from PVT block - */ - std::map get_beidou_dnav_ephemeris_map() const; - - /*! - * \brief Get latest set of BeiDou DNAV almanac from PVT block - */ - std::map get_beidou_dnav_almanac_map() const; - - /*! - * \brief Clear all ephemeris information and the almanacs for GPS and Galileo - */ - void clear_ephemeris(); - - /*! - * \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available - */ - bool get_latest_PVT(double* longitude_deg, - double* latitude_deg, - double* height_m, - double* ground_speed_kmh, - double* course_over_ground_deg, - time_t* UTC_time) const; - - int work(int noutput_items, gr_vector_const_void_star& input_items, - gr_vector_void_star& output_items); //!< PVT Signal Processing }; #endif diff --git a/src/algorithms/PVT/libs/geojson_printer.h b/src/algorithms/PVT/libs/geojson_printer.h index a74a9599d..8242fb634 100644 --- a/src/algorithms/PVT/libs/geojson_printer.h +++ b/src/algorithms/PVT/libs/geojson_printer.h @@ -47,18 +47,18 @@ class Pvt_Solution; */ class GeoJSON_Printer { -private: - std::ofstream geojson_file; - bool first_pos; - std::string filename_; - std::string geojson_base_path; - public: GeoJSON_Printer(const std::string& base_path = "."); ~GeoJSON_Printer(); bool set_headers(const std::string& filename, bool time_tag_name = true); bool print_position(const std::shared_ptr& position, bool print_average_values); bool close_file(); + +private: + std::ofstream geojson_file; + bool first_pos; + std::string filename_; + std::string geojson_base_path; }; #endif diff --git a/src/algorithms/PVT/libs/gpx_printer.h b/src/algorithms/PVT/libs/gpx_printer.h index e78269f8c..a0906cb09 100644 --- a/src/algorithms/PVT/libs/gpx_printer.h +++ b/src/algorithms/PVT/libs/gpx_printer.h @@ -47,19 +47,19 @@ class Rtklib_Solver; */ class Gpx_Printer { -private: - std::ofstream gpx_file; - bool positions_printed; - std::string gpx_filename; - std::string indent; - std::string gpx_base_path; - public: Gpx_Printer(const std::string& base_path = "."); ~Gpx_Printer(); bool set_headers(const std::string& filename, bool time_tag_name = true); bool print_position(const std::shared_ptr& position, bool print_average_values); bool close_file(); + +private: + std::ofstream gpx_file; + bool positions_printed; + std::string gpx_filename; + std::string indent; + std::string gpx_base_path; }; #endif diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index 4409d4bba..a1ab64191 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h @@ -48,14 +48,6 @@ */ class Hybrid_Ls_Pvt : public Ls_Pvt { -private: - int count_valid_position; - bool d_flag_dump_enabled; - std::string d_dump_filename; - std::ofstream d_dump_file; - int d_nchannels; // Number of available channels for positioning - double d_galileo_current_time; - public: Hybrid_Ls_Pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file); ~Hybrid_Ls_Pvt(); @@ -75,6 +67,14 @@ public: Gps_CNAV_Iono gps_cnav_iono; Gps_CNAV_Utc_Model gps_cnav_utc_model; + +private: + int count_valid_position; + bool d_flag_dump_enabled; + std::string d_dump_filename; + std::ofstream d_dump_file; + int d_nchannels; // Number of available channels for positioning + double d_galileo_current_time; }; #endif diff --git a/src/algorithms/PVT/libs/kml_printer.h b/src/algorithms/PVT/libs/kml_printer.h index 6295242f6..71b27de43 100644 --- a/src/algorithms/PVT/libs/kml_printer.h +++ b/src/algorithms/PVT/libs/kml_printer.h @@ -46,6 +46,13 @@ class Rtklib_Solver; */ class Kml_Printer { +public: + Kml_Printer(const std::string& base_path = std::string(".")); + ~Kml_Printer(); + bool set_headers(const std::string& filename, bool time_tag_name = true); + bool print_position(const std::shared_ptr& position, bool print_average_values); + bool close_file(); + private: std::ofstream kml_file; std::ofstream tmp_file; @@ -55,13 +62,6 @@ private: std::string tmp_file_str; unsigned int point_id; std::string indent; - -public: - Kml_Printer(const std::string& base_path = std::string(".")); - ~Kml_Printer(); - bool set_headers(const std::string& filename, bool time_tag_name = true); - bool print_position(const std::shared_ptr& position, bool print_average_values); - bool close_file(); }; #endif diff --git a/src/algorithms/PVT/libs/ls_pvt.h b/src/algorithms/PVT/libs/ls_pvt.h index 78ad9deef..2e7226798 100644 --- a/src/algorithms/PVT/libs/ls_pvt.h +++ b/src/algorithms/PVT/libs/ls_pvt.h @@ -41,12 +41,6 @@ */ class Ls_Pvt : public Pvt_Solution { -private: - /*! - * \brief Computes the Lorentz inner product between two vectors - */ - double lorentz(const arma::vec& x, const arma::vec& y); - public: Ls_Pvt(); @@ -59,6 +53,12 @@ public: * \brief Computes the Weighted Least Squares position solution */ arma::vec leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec); + +private: + /*! + * \brief Computes the Lorentz inner product between two vectors + */ + double lorentz(const arma::vec& x, const arma::vec& y); }; #endif diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 7afd074d2..86b1d72f1 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -44,32 +44,6 @@ */ class Pvt_Solution { -private: - double d_rx_dt_s; // RX time offset [s] - - double d_latitude_d; // RX position Latitude WGS84 [deg] - double d_longitude_d; // RX position Longitude WGS84 [deg] - double d_height_m; // RX position height WGS84 [m] - double d_speed_over_ground_m_s; // RX speed over ground [m/s] - double d_course_over_ground_d; // RX course over ground [deg] - - double d_avg_latitude_d; // Averaged latitude in degrees - double d_avg_longitude_d; // Averaged longitude in degrees - double d_avg_height_m; // Averaged height [m] - - bool b_valid_position; - - std::deque d_hist_latitude_d; - std::deque d_hist_longitude_d; - std::deque d_hist_height_m; - - bool d_flag_averaging; - int d_averaging_depth; // Length of averaging window - - arma::vec d_rx_pos; - boost::posix_time::ptime d_position_UTC_time; - int d_valid_observations; - public: Pvt_Solution(); @@ -111,46 +85,73 @@ public: arma::vec rotateSatellite(double traveltime, const arma::vec &X_sat); /*! - * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical - * coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid. - * - * \param[in] X [m] Cartesian coordinate - * \param[in] Y [m] Cartesian coordinate - * \param[in] Z [m] Cartesian coordinate - * \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates: - * 0 - International Ellipsoid 1924. - * 1 - International Ellipsoid 1967. - * 2 - World Geodetic System 1972. - * 3 - Geodetic Reference System 1980. - * 4 - World Geodetic System 1984. - * - */ + * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical + * coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid. + * + * \param[in] X [m] Cartesian coordinate + * \param[in] Y [m] Cartesian coordinate + * \param[in] Z [m] Cartesian coordinate + * \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates: + * 0 - International Ellipsoid 1924. + * 1 - International Ellipsoid 1967. + * 2 - World Geodetic System 1972. + * 3 - Geodetic Reference System 1980. + * 4 - World Geodetic System 1984. + * + */ int cart2geo(double X, double Y, double Z, int elipsoid_selection); /*! - * \brief Tropospheric correction - * - * \param[in] sinel - sin of elevation angle of satellite - * \param[in] hsta_km - height of station in km - * \param[in] p_mb - atmospheric pressure in mb at height hp_km - * \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km - * \param[in] hum - humidity in % at height hhum_km - * \param[in] hp_km - height of pressure measurement in km - * \param[in] htkel_km - height of temperature measurement in km - * \param[in] hhum_km - height of humidity measurement in km - * - * \param[out] ddr_m - range correction (meters) - * - * - * Reference: - * Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric - * Refraction Correction Model. Paper presented at the - * American Geophysical Union Annual Fall Meeting, San - * Francisco, December 12-17 - * - * Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre - */ + * \brief Tropospheric correction + * + * \param[in] sinel - sin of elevation angle of satellite + * \param[in] hsta_km - height of station in km + * \param[in] p_mb - atmospheric pressure in mb at height hp_km + * \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km + * \param[in] hum - humidity in % at height hhum_km + * \param[in] hp_km - height of pressure measurement in km + * \param[in] htkel_km - height of temperature measurement in km + * \param[in] hhum_km - height of humidity measurement in km + * + * \param[out] ddr_m - range correction (meters) + * + * + * Reference: + * Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric + * Refraction Correction Model. Paper presented at the + * American Geophysical Union Annual Fall Meeting, San + * Francisco, December 12-17 + * + * Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre + */ int tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km); + +private: + double d_rx_dt_s; // RX time offset [s] + + double d_latitude_d; // RX position Latitude WGS84 [deg] + double d_longitude_d; // RX position Longitude WGS84 [deg] + double d_height_m; // RX position height WGS84 [m] + double d_speed_over_ground_m_s; // RX speed over ground [m/s] + double d_course_over_ground_d; // RX course over ground [deg] + + double d_avg_latitude_d; // Averaged latitude in degrees + double d_avg_longitude_d; // Averaged longitude in degrees + double d_avg_height_m; // Averaged height [m] + + bool b_valid_position; + + std::deque d_hist_latitude_d; + std::deque d_hist_longitude_d; + std::deque d_hist_height_m; + + bool d_flag_averaging; + int d_averaging_depth; // Length of averaging window + + arma::vec d_rx_pos; + boost::posix_time::ptime d_position_UTC_time; + int d_valid_observations; + }; #endif diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index b13e2446f..8e1b94cc7 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -88,18 +88,6 @@ */ class Rtklib_Solver : public Pvt_Solution { -private: - rtk_t rtk_; - std::string d_dump_filename; - std::ofstream d_dump_file; - bool save_matfile(); - - bool d_flag_dump_enabled; - bool d_flag_dump_mat_enabled; - int d_nchannels; // Number of available channels for positioning - std::array dop_; - Monitor_Pvt monitor_pvt; - public: Rtklib_Solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, const rtk_t& rtk); ~Rtklib_Solver(); @@ -139,6 +127,17 @@ public: std::map beidou_dnav_almanac_map; int count_valid_position; + +private: + rtk_t rtk_; + std::string d_dump_filename; + std::ofstream d_dump_file; + bool save_matfile(); + bool d_flag_dump_enabled; + bool d_flag_dump_mat_enabled; + int d_nchannels; // Number of available channels for positioning + std::array dop_; + Monitor_Pvt monitor_pvt; }; #endif diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.h b/src/algorithms/channel/libs/channel_msg_receiver_cc.h index 40c542dde..7f305f7f7 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.h +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.h @@ -47,15 +47,15 @@ channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr d_channel_fsm; bool d_repeat; // todo: change FSM to include repeat value friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); void msg_handler_events(pmt::pmt_t msg); channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat); - -public: - ~channel_msg_receiver_cc(); //!< Default destructor }; #endif diff --git a/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_byte.h b/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_byte.h index 437d60d4e..cf033c608 100644 --- a/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_byte.h +++ b/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_byte.h @@ -47,15 +47,14 @@ interleaved_byte_to_complex_byte_sptr make_interleaved_byte_to_complex_byte(); */ class interleaved_byte_to_complex_byte : public gr::sync_decimator { -private: - friend interleaved_byte_to_complex_byte_sptr make_interleaved_byte_to_complex_byte(); - public: - interleaved_byte_to_complex_byte(); - int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend interleaved_byte_to_complex_byte_sptr make_interleaved_byte_to_complex_byte(); + interleaved_byte_to_complex_byte(); }; #endif diff --git a/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_short.h b/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_short.h index 0b0fbdf1b..e37bb4542 100644 --- a/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_short.h +++ b/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_short.h @@ -46,15 +46,14 @@ interleaved_byte_to_complex_short_sptr make_interleaved_byte_to_complex_short(); */ class interleaved_byte_to_complex_short : public gr::sync_decimator { -private: - friend interleaved_byte_to_complex_short_sptr make_interleaved_byte_to_complex_short(); - public: - interleaved_byte_to_complex_short(); - int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend interleaved_byte_to_complex_short_sptr make_interleaved_byte_to_complex_short(); + interleaved_byte_to_complex_short(); }; -#endif +#endif // GNSS_SDR_INTERLEAVED_BYTE_TO_COMPLEX_SHORT_H_ diff --git a/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_short_to_complex_short.h b/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_short_to_complex_short.h index e31b6ac98..7aec91324 100644 --- a/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_short_to_complex_short.h +++ b/src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_short_to_complex_short.h @@ -46,15 +46,14 @@ interleaved_short_to_complex_short_sptr make_interleaved_short_to_complex_short( */ class interleaved_short_to_complex_short : public gr::sync_decimator { -private: - friend interleaved_short_to_complex_short_sptr make_interleaved_short_to_complex_short(); - public: - interleaved_short_to_complex_short(); - int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend interleaved_short_to_complex_short_sptr make_interleaved_short_to_complex_short(); + interleaved_short_to_complex_short(); }; -#endif +#endif // GNSS_SDR_INTERLEAVED_SHORT_TO_COMPLEX_SHORT_H_ diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h index 6fc126272..55e9fcc27 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h @@ -43,17 +43,15 @@ beamformer_sptr make_beamformer(); */ class beamformer : public gr::sync_block { -private: - friend beamformer_sptr - make_beamformer_sptr(); - - gr_complex *weight_vector; - public: beamformer(); ~beamformer(); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend beamformer_sptr make_beamformer_sptr(); + gr_complex *weight_vector; }; #endif diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h index 186d1feb1..4f898b154 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h @@ -47,9 +47,19 @@ notch_sptr make_notch_filter(float pfa, float p_c_factor, /*! * \brief This class implements a real-time software-defined multi state notch filter */ - class Notch : public gr::block { +public: + Notch(float pfa, float p_c_factor, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); + + ~Notch(); + + void forecast(int noutput_items, gr_vector_int &ninput_items_required); + + int general_work(int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + private: float pfa; float noise_pow_est; @@ -67,17 +77,6 @@ private: float *angle_; float *power_spect; std::unique_ptr d_fft; - -public: - Notch(float pfa, float p_c_factor, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); - - ~Notch(); - - void forecast(int noutput_items, gr_vector_int &ninput_items_required); - - int general_work(int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; -#endif //GNSS_SDR_NOTCH_H_ +#endif // GNSS_SDR_NOTCH_H_ diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h index 111dd65b0..0a7114240 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h @@ -46,9 +46,19 @@ notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int32_t leng /*! * \brief This class implements a real-time software-defined multi state notch filter light version */ - class NotchLite : public gr::block { +public: + NotchLite(float p_c_factor, float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset, int32_t n_segments_coeff); + + ~NotchLite(); + + void forecast(int noutput_items, gr_vector_int &ninput_items_required); + + int general_work(int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + private: int32_t length_; int32_t n_segments; @@ -70,17 +80,6 @@ private: float angle2; float *power_spect; std::unique_ptr d_fft; - -public: - NotchLite(float p_c_factor, float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset, int32_t n_segments_coeff); - - ~NotchLite(); - - void forecast(int noutput_items, gr_vector_int &ninput_items_required); - - int general_work(int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; -#endif //GNSS_SDR_NOTCH_LITE_H_ +#endif // GNSS_SDR_NOTCH_LITE_H_ diff --git a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h index af5584463..52abef24f 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h @@ -44,6 +44,16 @@ pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int32_t length_, int32_ class pulse_blanking_cc : public gr::block { +public: + pulse_blanking_cc(float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); + + ~pulse_blanking_cc(); + + void forecast(int noutput_items, gr_vector_int &ninput_items_required); + + int general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + private: int32_t length_; int32_t n_segments; @@ -55,16 +65,6 @@ private: float thres_; float pfa; gr_complex *zeros_; - -public: - pulse_blanking_cc(float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); - - ~pulse_blanking_cc(); - - int general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - void forecast(int noutput_items, gr_vector_int &ninput_items_required); }; -#endif +#endif // GNSS_SDR_PULSE_BLANKING_H_ diff --git a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.h b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.h index ac93ab82b..e7ceb9683 100644 --- a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.h +++ b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.h @@ -38,9 +38,10 @@ class direct_resampler_conditioner_cb; using direct_resampler_conditioner_cb_sptr = boost::shared_ptr; -direct_resampler_conditioner_cb_sptr -direct_resampler_make_conditioner_cb(double sample_freq_in, +direct_resampler_conditioner_cb_sptr direct_resampler_make_conditioner_cb( + double sample_freq_in, double sample_freq_out); + /*! * \brief This class implements a direct resampler conditioner for std::complex * @@ -48,20 +49,6 @@ direct_resampler_make_conditioner_cb(double sample_freq_in, */ class direct_resampler_conditioner_cb : public gr::block { -private: - friend direct_resampler_conditioner_cb_sptr - direct_resampler_make_conditioner_cb(double sample_freq_in, - double sample_freq_out); - - double d_sample_freq_in; - double d_sample_freq_out; - uint32_t d_phase; - uint32_t d_lphase; - uint32_t d_phase_step; - - direct_resampler_conditioner_cb(double sample_freq_in, - double sample_freq_out); - public: ~direct_resampler_conditioner_cb(); @@ -80,6 +67,21 @@ public: int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend direct_resampler_conditioner_cb_sptr direct_resampler_make_conditioner_cb( + double sample_freq_in, + double sample_freq_out); + + direct_resampler_conditioner_cb( + double sample_freq_in, + double sample_freq_out); + + double d_sample_freq_in; + double d_sample_freq_out; + uint32_t d_phase; + uint32_t d_lphase; + uint32_t d_phase_step; }; #endif /* GNSS_SDR_DIRECT_RESAMPLER_CONDITIONER_CS_H */ diff --git a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.h b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.h index 945f797ce..6469aada5 100644 --- a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.h +++ b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.h @@ -43,9 +43,11 @@ #include class direct_resampler_conditioner_cc; + using direct_resampler_conditioner_cc_sptr = boost::shared_ptr; -direct_resampler_conditioner_cc_sptr -direct_resampler_make_conditioner_cc(double sample_freq_in, + +direct_resampler_conditioner_cc_sptr direct_resampler_make_conditioner_cc( + double sample_freq_in, double sample_freq_out); /*! @@ -55,18 +57,6 @@ direct_resampler_make_conditioner_cc(double sample_freq_in, */ class direct_resampler_conditioner_cc : public gr::block { -private: - friend direct_resampler_conditioner_cc_sptr - direct_resampler_make_conditioner_cc(double sample_freq_in, - double sample_freq_out); - double d_sample_freq_in; //! Specifies the sampling frequency of the input signal - double d_sample_freq_out; //! Specifies the sampling frequency of the output signal - uint32_t d_phase; - uint32_t d_lphase; - uint32_t d_phase_step; - direct_resampler_conditioner_cc(double sample_freq_in, - double sample_freq_out); - public: ~direct_resampler_conditioner_cc(); inline unsigned int sample_freq_in() const @@ -84,6 +74,21 @@ public: int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend direct_resampler_conditioner_cc_sptr direct_resampler_make_conditioner_cc( + double sample_freq_in, + double sample_freq_out); + + direct_resampler_conditioner_cc( + double sample_freq_in, + double sample_freq_out); + + double d_sample_freq_in; // Sampling frequency of the input signal + double d_sample_freq_out; // Sampling frequency of the output signal + uint32_t d_phase; + uint32_t d_lphase; + uint32_t d_phase_step; }; #endif /* GNSS_SDR_DIRECT_RESAMPLER_CONDITIONER_CC_H */ diff --git a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.h b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.h index 9d904abfb..5b643be93 100644 --- a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.h +++ b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.h @@ -38,9 +38,10 @@ class direct_resampler_conditioner_cs; using direct_resampler_conditioner_cs_sptr = boost::shared_ptr; -direct_resampler_conditioner_cs_sptr -direct_resampler_make_conditioner_cs(double sample_freq_in, +direct_resampler_conditioner_cs_sptr direct_resampler_make_conditioner_cs( + double sample_freq_in, double sample_freq_out); + /*! * \brief This class implements a direct resampler conditioner for std::complex * @@ -48,20 +49,6 @@ direct_resampler_make_conditioner_cs(double sample_freq_in, */ class direct_resampler_conditioner_cs : public gr::block { -private: - friend direct_resampler_conditioner_cs_sptr - direct_resampler_make_conditioner_cs(double sample_freq_in, - double sample_freq_out); - - double d_sample_freq_in; - double d_sample_freq_out; - uint32_t d_phase; - uint32_t d_lphase; - uint32_t d_phase_step; - - direct_resampler_conditioner_cs(double sample_freq_in, - double sample_freq_out); - public: ~direct_resampler_conditioner_cs(); @@ -80,6 +67,21 @@ public: int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend direct_resampler_conditioner_cs_sptr direct_resampler_make_conditioner_cs( + double sample_freq_in, + double sample_freq_out); + + direct_resampler_conditioner_cs( + double sample_freq_in, + double sample_freq_out); + + double d_sample_freq_in; + double d_sample_freq_out; + uint32_t d_phase; + uint32_t d_lphase; + uint32_t d_phase_step; }; #endif /* GNSS_SDR_DIRECT_RESAMPLER_CONDITIONER_CS_H */ diff --git a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.h b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.h index 2201a849c..a6fb316bf 100644 --- a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.h +++ b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.h @@ -60,11 +60,19 @@ using signal_generator_c_sptr = boost::shared_ptr; * constructor is private. signal_make_generator_c is the public * interface for creating new instances. */ -signal_generator_c_sptr -signal_make_generator_c(std::vector signal1, std::vector system, const std::vector &PRN, - const std::vector &CN0_dB, const std::vector &doppler_Hz, - const std::vector &delay_chips, const std::vector &delay_sec, bool data_flag, bool noise_flag, - unsigned int fs_in, unsigned int vector_length, float BW_BB); +signal_generator_c_sptr signal_make_generator_c( + std::vector signal1, + std::vector system, + const std::vector &PRN, + const std::vector &CN0_dB, + const std::vector &doppler_Hz, + const std::vector &delay_chips, + const std::vector &delay_sec, + bool data_flag, + bool noise_flag, + unsigned int fs_in, + unsigned int vector_length, + float BW_BB); /*! * \brief This class generates synthesized GNSS signal. @@ -74,23 +82,46 @@ signal_make_generator_c(std::vector signal1, std::vector signal1, + std::vector system, + const std::vector &PRN, + const std::vector &CN0_dB, + const std::vector &doppler_Hz, + const std::vector &delay_chips, + const std::vector &delay_sec, + bool data_flag, + bool noise_flag, + unsigned int fs_in, + unsigned int vector_length, + float BW_BB); - /* Create the signal_generator_c object*/ - friend signal_generator_c_sptr - signal_make_generator_c(std::vector signal1, std::vector system, const std::vector &PRN, - const std::vector &CN0_dB, const std::vector &doppler_Hz, - const std::vector &delay_chips, const std::vector &delay_sec, bool data_flag, bool noise_flag, - unsigned int fs_in, unsigned int vector_length, float BW_BB); - - signal_generator_c(std::vector signal1, std::vector system, const std::vector &PRN, - std::vector CN0_dB, std::vector doppler_Hz, - std::vector delay_chips, std::vector delay_sec, bool data_flag, bool noise_flag, - unsigned int fs_in, unsigned int vector_length, float BW_BB); + signal_generator_c( + std::vector signal1, + std::vector system, + const std::vector &PRN, + std::vector CN0_dB, + std::vector doppler_Hz, + std::vector delay_chips, + std::vector delay_sec, + bool data_flag, + bool noise_flag, + unsigned int fs_in, + unsigned int vector_length, + float BW_BB); void init(); + void generate_codes(); std::vector signal_; @@ -106,7 +137,6 @@ private: unsigned int num_sats_; unsigned int vector_length_; float BW_BB_; - std::vector samples_per_code_; std::vector num_of_codes_per_vector_; std::vector data_bit_duration_ms_; @@ -116,28 +146,15 @@ private: std::vector current_data_bit_int_; std::vector data_modulation_; std::vector pilot_modulation_; - boost::scoped_array sampled_code_data_; boost::scoped_array sampled_code_pilot_; - //gr::random *random_; gr_complex *complex_phase_; - unsigned int work_counter_; std::random_device r; std::default_random_engine e1; std::default_random_engine e2; std::uniform_int_distribution uniform_dist; std::normal_distribution normal_dist; - -public: - ~signal_generator_c(); // public destructor - - // Where all the action really happens - - int general_work(int noutput_items, - gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; #endif /* GNSS_SDR_SIGNAL_GENERATOR_C_H */ diff --git a/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.h b/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.h index ee915a662..4e8196fcb 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.h +++ b/src/algorithms/signal_source/gnuradio_blocks/gr_complex_ip_packet_source.h @@ -45,42 +45,6 @@ class Gr_Complex_Ip_Packet_Source : virtual public gr::sync_block { -private: - boost::mutex d_mutex; - pcap_t *descr; //ethernet pcap device descriptor - - char *fifo_buff; - - int fifo_read_ptr; - int fifo_write_ptr; - int fifo_items; - int d_sock_raw; - int d_udp_port; - struct sockaddr_in si_me; - std::string d_src_device; - std::string d_origin_address; - int d_udp_payload_size; - bool d_fifo_full; - - int d_n_baseband_channels; - int d_wire_sample_type; - int d_bytes_per_sample; - size_t d_item_size; - bool d_IQ_swap; - - boost::thread *d_pcap_thread; - /*! - * \brief - * Opens the ethernet device using libpcap raw capture mode - * If any of these fail, the fuction retuns the error and exits. - */ - bool open(); - - void demux_samples(gr_vector_void_star output_items, int num_samples_readed); - void my_pcap_loop_thread(pcap_t *pcap_handle); - void pcap_callback(u_char *args, const struct pcap_pkthdr *pkthdr, const u_char *packet); - static void static_pcap_callback(u_char *args, const struct pcap_pkthdr *pkthdr, const u_char *packet); - public: typedef boost::shared_ptr sptr; static sptr make(std::string src_device, @@ -101,15 +65,46 @@ public: bool IQ_swap_); ~Gr_Complex_Ip_Packet_Source(); + // Called by gnuradio to enable drivers, etc for i/o devices. + bool start(); + + // Called by gnuradio to disable drivers, etc for i/o devices. + bool stop(); + // Where all the action really happens int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - // Called by gnuradio to enable drivers, etc for i/o devices. - bool start(); - // Called by gnuradio to disable drivers, etc for i/o devices. - bool stop(); +private: + boost::mutex d_mutex; + pcap_t *descr; //ethernet pcap device descriptor + char *fifo_buff; + int fifo_read_ptr; + int fifo_write_ptr; + int fifo_items; + int d_sock_raw; + int d_udp_port; + struct sockaddr_in si_me; + std::string d_src_device; + std::string d_origin_address; + int d_udp_payload_size; + bool d_fifo_full; + int d_n_baseband_channels; + int d_wire_sample_type; + int d_bytes_per_sample; + size_t d_item_size; + bool d_IQ_swap; + boost::thread *d_pcap_thread; + void demux_samples(gr_vector_void_star output_items, int num_samples_readed); + void my_pcap_loop_thread(pcap_t *pcap_handle); + void pcap_callback(u_char *args, const struct pcap_pkthdr *pkthdr, const u_char *packet); + static void static_pcap_callback(u_char *args, const struct pcap_pkthdr *pkthdr, const u_char *packet); + /* + * Opens the ethernet device using libpcap raw capture mode + * If any of these fail, the fuction retuns the error and exits. + */ + bool open(); }; #endif /* GNSS_SDR_GR_COMPLEX_IP_PACKET_SOURCE_H */ diff --git a/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.h b/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.h index a0ec9432f..a09c0f3a8 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.h +++ b/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.h @@ -42,16 +42,34 @@ class labsat23_source; using labsat23_source_sptr = boost::shared_ptr; -labsat23_source_sptr labsat23_make_source_sptr(const char *signal_file_basename, int channel_selector, gr::msg_queue::sptr queue); +labsat23_source_sptr labsat23_make_source_sptr( + const char *signal_file_basename, + int channel_selector, + gr::msg_queue::sptr queue); /*! * \brief This class implements conversion between Labsat2 and 3 format byte packet samples to gr_complex */ class labsat23_source : public gr::block { +public: + ~labsat23_source(); + + int general_work(int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + private: - friend labsat23_source_sptr labsat23_make_source_sptr(const char *signal_file_basename, int channel_selector, gr::msg_queue::sptr queue); - labsat23_source(const char *signal_file_basename, int channel_selector, gr::msg_queue::sptr queue); + friend labsat23_source_sptr labsat23_make_source_sptr( + const char *signal_file_basename, + int channel_selector, + gr::msg_queue::sptr queue); + + labsat23_source(const char *signal_file_basename, + int channel_selector, + gr::msg_queue::sptr queue); + std::string generate_filename(); void decode_samples_one_channel(int16_t input_short, gr_complex *out, int type); int getBit(uint8_t byte, int position); @@ -65,13 +83,6 @@ private: uint8_t d_ref_clock; uint8_t d_bits_per_sample; gr::msg_queue::sptr d_queue; - -public: - ~labsat23_source(); - int general_work(int noutput_items, - gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; #endif diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_2bit_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_2bit_samples.h index 876a38d1d..f91e2d8d4 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_2bit_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_2bit_samples.h @@ -75,7 +75,8 @@ class unpack_2bit_samples; using unpack_2bit_samples_sptr = boost::shared_ptr; -unpack_2bit_samples_sptr make_unpack_2bit_samples(bool big_endian_bytes, +unpack_2bit_samples_sptr make_unpack_2bit_samples( + bool big_endian_bytes, size_t item_size, bool big_endian_items, bool reverse_interleaving = false); @@ -87,12 +88,25 @@ unpack_2bit_samples_sptr make_unpack_2bit_samples(bool big_endian_bytes, */ class unpack_2bit_samples : public gr::sync_interpolator { -private: - friend unpack_2bit_samples_sptr - make_unpack_2bit_samples_sptr(bool big_endian_bytes, +public: + ~unpack_2bit_samples(); + + unpack_2bit_samples(bool big_endian_bytes, size_t item_size, bool big_endian_items, bool reverse_interleaving); + + int work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + +private: + friend unpack_2bit_samples_sptr make_unpack_2bit_samples_sptr( + bool big_endian_bytes, + size_t item_size, + bool big_endian_items, + bool reverse_interleaving); + bool big_endian_bytes_; size_t item_size_; bool big_endian_items_; @@ -100,18 +114,6 @@ private: bool swap_endian_bytes_; bool reverse_interleaving_; std::vector work_buffer_; - -public: - unpack_2bit_samples(bool big_endian_bytes, - size_t item_size, - bool big_endian_items, - bool reverse_interleaving); - - ~unpack_2bit_samples(); - - int work(int noutput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; #endif diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_cpx_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_cpx_samples.h index 66ff922df..b399aba19 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_cpx_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_cpx_samples.h @@ -49,15 +49,15 @@ unpack_byte_2bit_cpx_samples_sptr make_unpack_byte_2bit_cpx_samples(); */ class unpack_byte_2bit_cpx_samples : public gr::sync_interpolator { -private: - friend unpack_byte_2bit_cpx_samples_sptr make_unpack_byte_2bit_cpx_samples_sptr(); - public: unpack_byte_2bit_cpx_samples(); ~unpack_byte_2bit_cpx_samples(); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend unpack_byte_2bit_cpx_samples_sptr make_unpack_byte_2bit_cpx_samples_sptr(); }; #endif diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_samples.h index 92a115adf..b9b86c503 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_2bit_samples.h @@ -45,16 +45,15 @@ unpack_byte_2bit_samples_sptr make_unpack_byte_2bit_samples(); */ class unpack_byte_2bit_samples : public gr::sync_interpolator { -private: - friend unpack_byte_2bit_samples_sptr - make_unpack_byte_2bit_samples_sptr(); - public: unpack_byte_2bit_samples(); ~unpack_byte_2bit_samples(); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend unpack_byte_2bit_samples_sptr make_unpack_byte_2bit_samples_sptr(); }; #endif diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_4bit_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_4bit_samples.h index 8b9ad7454..599590a2c 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_4bit_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_byte_4bit_samples.h @@ -47,15 +47,15 @@ unpack_byte_4bit_samples_sptr make_unpack_byte_4bit_samples(); */ class unpack_byte_4bit_samples : public gr::sync_interpolator { -private: - friend unpack_byte_4bit_samples_sptr make_unpack_byte_4bit_samples_sptr(); - public: unpack_byte_4bit_samples(); ~unpack_byte_4bit_samples(); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend unpack_byte_4bit_samples_sptr make_unpack_byte_4bit_samples_sptr(); }; #endif diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_intspir_1bit_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_intspir_1bit_samples.h index 7b8e7d7dc..7be626bf7 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_intspir_1bit_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_intspir_1bit_samples.h @@ -45,16 +45,15 @@ unpack_intspir_1bit_samples_sptr make_unpack_intspir_1bit_samples(); */ class unpack_intspir_1bit_samples : public gr::sync_interpolator { -private: - friend unpack_intspir_1bit_samples_sptr - make_unpack_intspir_1bit_samples_sptr(); - public: unpack_intspir_1bit_samples(); ~unpack_intspir_1bit_samples(); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + +private: + friend unpack_intspir_1bit_samples_sptr make_unpack_intspir_1bit_samples_sptr(); }; #endif diff --git a/src/algorithms/signal_source/libs/gnss_sdr_valve.h b/src/algorithms/signal_source/libs/gnss_sdr_valve.h index 76ddc9cd6..0ab0d08ab 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_valve.h +++ b/src/algorithms/signal_source/libs/gnss_sdr_valve.h @@ -41,11 +41,13 @@ #include // for size_t #include -boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, +boost::shared_ptr gnss_sdr_make_valve( + size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue); -boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, +boost::shared_ptr gnss_sdr_make_valve( + size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue, bool stop_flowgraph); @@ -56,29 +58,34 @@ boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, */ class Gnss_Sdr_Valve : public gr::sync_block { - friend boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, +public: + void open_valve(); + + int work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + +private: + friend boost::shared_ptr gnss_sdr_make_valve( + size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue); - friend boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, + + friend boost::shared_ptr gnss_sdr_make_valve( + size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue, bool stop_flowgraph); + Gnss_Sdr_Valve(size_t sizeof_stream_item, + uint64_t nitems, + gr::msg_queue::sptr queue, bool stop_flowgraph); + uint64_t d_nitems; uint64_t d_ncopied_items; gr::msg_queue::sptr d_queue; bool d_stop_flowgraph; bool d_open_valve; - -public: - Gnss_Sdr_Valve(size_t sizeof_stream_item, - uint64_t nitems, - gr::msg_queue::sptr queue, bool stop_flowgraph); - void open_valve(); - - int work(int noutput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; #endif /*GNSS_SDR_GNSS_SDR_VALVE_H_*/ diff --git a/src/algorithms/signal_source/libs/rtl_tcp_dongle_info.h b/src/algorithms/signal_source/libs/rtl_tcp_dongle_info.h index e7ac9eb94..16c634a9d 100644 --- a/src/algorithms/signal_source/libs/rtl_tcp_dongle_info.h +++ b/src/algorithms/signal_source/libs/rtl_tcp_dongle_info.h @@ -41,11 +41,6 @@ */ class Rtl_Tcp_Dongle_Info { -private: - char magic_[4]{}; - uint32_t tuner_type_; - uint32_t tuner_gain_count_; - public: enum { @@ -77,6 +72,11 @@ public: { return tuner_gain_count_; } + +private: + char magic_[4]{}; + uint32_t tuner_type_; + uint32_t tuner_gain_count_; }; diff --git a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h index 446cecfd8..21e7aa8b8 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h @@ -47,6 +47,15 @@ */ class Tracking_2nd_DLL_filter { +public: + void set_DLL_BW(float dll_bw_hz); //!< Set DLL filter bandwidth [Hz] + void set_pdi(float pdi_code); //!< Set Summation interval for code [s] + void initialize(); //!< Start tracking with acquisition information + float get_code_nco(float DLL_discriminator); //!< Numerically controlled oscillator + Tracking_2nd_DLL_filter(float pdi_code); + Tracking_2nd_DLL_filter(); + ~Tracking_2nd_DLL_filter(); + private: // PLL filter parameters float d_tau1_code = 0.0; @@ -57,15 +66,6 @@ private: float d_old_code_error = 0.0; float d_old_code_nco = 0.0; void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); - -public: - void set_DLL_BW(float dll_bw_hz); //! Set DLL filter bandwidth [Hz] - void set_pdi(float pdi_code); //! Set Summation interval for code [s] - void initialize(); //! Start tracking with acquisition information - float get_code_nco(float DLL_discriminator); //! Numerically controlled oscillator - Tracking_2nd_DLL_filter(float pdi_code); - Tracking_2nd_DLL_filter(); - ~Tracking_2nd_DLL_filter(); }; #endif diff --git a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h index 9a1e9b700..53e2349cf 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h @@ -46,28 +46,25 @@ */ class Tracking_2nd_PLL_filter { -private: - // PLL filter parameters - float d_tau1_carr = 0.0; - float d_tau2_carr = 0.0; - float d_pdi_carr = 0.0; - - float d_pllnoisebandwidth = 0.0; - float d_plldampingratio = 0.0; - - float d_old_carr_error = 0.0; - float d_old_carr_nco = 0.0; - - void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); - public: - void set_PLL_BW(float pll_bw_hz); //! Set PLL loop bandwidth [Hz] - void set_pdi(float pdi_carr); //! Set Summation interval for code [s] + void set_PLL_BW(float pll_bw_hz); //!< Set PLL loop bandwidth [Hz] + void set_pdi(float pdi_carr); //!< Set Summation interval for code [s] void initialize(); float get_carrier_nco(float PLL_discriminator); Tracking_2nd_PLL_filter(float pdi_carr); Tracking_2nd_PLL_filter(); ~Tracking_2nd_PLL_filter(); + +private: + // PLL filter parameters + float d_tau1_carr = 0.0; + float d_tau2_carr = 0.0; + float d_pdi_carr = 0.0; + float d_pllnoisebandwidth = 0.0; + float d_plldampingratio = 0.0; + float d_old_carr_error = 0.0; + float d_old_carr_nco = 0.0; + void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); }; #endif diff --git a/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h b/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h index f2d903513..40785ea60 100644 --- a/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h @@ -36,6 +36,13 @@ */ class Tracking_FLL_PLL_filter { +public: + void set_params(float fll_bw_hz, float pll_bw_hz, int order); + void initialize(float d_acq_carrier_doppler_hz); + float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s); + Tracking_FLL_PLL_filter(); + ~Tracking_FLL_PLL_filter(); + private: // FLL + PLL filter parameters int d_order; @@ -49,13 +56,6 @@ private: float d_pll_w0p2; float d_pll_b3; float d_pll_w0p; - -public: - void set_params(float fll_bw_hz, float pll_bw_hz, int order); - void initialize(float d_acq_carrier_doppler_hz); - float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s); - Tracking_FLL_PLL_filter(); - ~Tracking_FLL_PLL_filter(); }; #endif diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index cb626c93f..0151f3c5a 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -43,6 +43,27 @@ */ class Tracking_loop_filter { +public: + float get_noise_bandwidth(void) const; + float get_update_interval(void) const; + bool get_include_last_integrator(void) const; + int get_order(void) const; + + void set_noise_bandwidth(float noise_bandwidth); + void set_update_interval(float update_interval); + void set_include_last_integrator(bool include_last_integrator); + void set_order(int loop_order); + + void initialize(float initial_output = 0.0); + float apply(float current_input); + + Tracking_loop_filter(float update_interval, float noise_bandwidth, + int loop_order = 2, + bool include_last_integrator = false); + + Tracking_loop_filter(); + ~Tracking_loop_filter(); + private: // Store the last inputs and outputs: std::vector d_inputs; @@ -71,27 +92,6 @@ private: // Compute the filter coefficients: void update_coefficients(void); - -public: - float get_noise_bandwidth(void) const; - float get_update_interval(void) const; - bool get_include_last_integrator(void) const; - int get_order(void) const; - - void set_noise_bandwidth(float noise_bandwidth); - void set_update_interval(float update_interval); - void set_include_last_integrator(bool include_last_integrator); - void set_order(int loop_order); - - void initialize(float initial_output = 0.0); - float apply(float current_input); - - Tracking_loop_filter(float update_interval, float noise_bandwidth, - int loop_order = 2, - bool include_last_integrator = false); - - Tracking_loop_filter(); - ~Tracking_loop_filter(); }; #endif diff --git a/src/core/libs/gnss_sdr_fpga_sample_counter.h b/src/core/libs/gnss_sdr_fpga_sample_counter.h index 70df49283..18b232869 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.h +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.h @@ -47,6 +47,13 @@ gnss_sdr_fpga_sample_counter_sptr gnss_sdr_make_fpga_sample_counter(double _fs, class gnss_sdr_fpga_sample_counter : public gr::block { +public: + ~gnss_sdr_fpga_sample_counter(); + int general_work(int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + private: friend gnss_sdr_fpga_sample_counter_sptr gnss_sdr_make_fpga_sample_counter(double _fs, int32_t _interval_ms); gnss_sdr_fpga_sample_counter(double _fs, int32_t _interval_ms); @@ -80,13 +87,6 @@ private: volatile uint32_t *map_base; // driver memory map std::string device_name = "/dev/uio2"; // HW device name bool is_open; - -public: - ~gnss_sdr_fpga_sample_counter(); - int general_work(int noutput_items, - gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; #endif // GNSS_SDR_GNSS_SDR_FPGA_SAMPLE_COUNTER_H_ diff --git a/src/core/libs/gnss_sdr_sample_counter.h b/src/core/libs/gnss_sdr_sample_counter.h index af36ba869..4fbf1f103 100644 --- a/src/core/libs/gnss_sdr_sample_counter.h +++ b/src/core/libs/gnss_sdr_sample_counter.h @@ -50,9 +50,22 @@ gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter( class gnss_sdr_sample_counter : public gr::sync_decimator { +public: + ~gnss_sdr_sample_counter(); + int work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + private: - friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int32_t _interval_ms, size_t _size); - gnss_sdr_sample_counter(double _fs, int32_t _interval_ms, size_t _size); + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter( + double _fs, + int32_t _interval_ms, + size_t _size); + + gnss_sdr_sample_counter(double _fs, + int32_t _interval_ms, + size_t _size); + uint32_t samples_per_output; double fs; uint64_t sample_counter; @@ -67,12 +80,6 @@ private: uint32_t current_days; // Receiver time in days since the beginning of the run int32_t report_interval_ms; bool flag_enable_send_msg; - -public: - ~gnss_sdr_sample_counter(); - int work(int noutput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); }; #endif /*GNSS_SDR_GNSS_SDR_SAMPLE_COUNTER_H_*/ diff --git a/src/core/libs/gnss_sdr_supl_client.h b/src/core/libs/gnss_sdr_supl_client.h index 478fe7127..ebae162ee 100644 --- a/src/core/libs/gnss_sdr_supl_client.h +++ b/src/core/libs/gnss_sdr_supl_client.h @@ -62,19 +62,10 @@ extern "C" */ class Gnss_Sdr_Supl_Client { -private: - // GSM CELL INFO - int mcc; - int mns; - int lac; - int ci; - // assistance protocol structure - supl_ctx_t ctx{}; - // assistance data - supl_assist_t assist{}; - bool read_gal_almanac_from_gsa(const std::string& file_name); - public: + Gnss_Sdr_Supl_Client(); + ~Gnss_Sdr_Supl_Client(); + // SUPL SERVER INFO std::string server_name; int server_port; @@ -113,6 +104,7 @@ public: * \return Error code -> 0 no errors. */ int get_assistance(int i_mcc, int i_mns, int i_lac, int i_ci); + /* * \brief Read the received SUPL data and stores it into the corresponding class members (gps_ephemeris_map, gps_almanac_map, gps_iono, gps_time, gps_utc, gps_acq_map, and gps_ref_loc) * @@ -270,8 +262,17 @@ public: */ void print_assistance(); - Gnss_Sdr_Supl_Client(); - ~Gnss_Sdr_Supl_Client(); +private: + // GSM CELL INFO + int mcc; + int mns; + int lac; + int ci; + // assistance protocol structure + supl_ctx_t ctx{}; + // assistance data + supl_assist_t assist{}; + bool read_gal_almanac_from_gsa(const std::string& file_name); }; #endif diff --git a/src/core/libs/gnss_sdr_time_counter.h b/src/core/libs/gnss_sdr_time_counter.h index 76a7e09ed..bbf17d9fc 100644 --- a/src/core/libs/gnss_sdr_time_counter.h +++ b/src/core/libs/gnss_sdr_time_counter.h @@ -45,6 +45,13 @@ gnss_sdr_time_counter_sptr gnss_sdr_make_time_counter(); class gnss_sdr_time_counter : public gr::block { +public: + ~gnss_sdr_time_counter(); + int general_work(int noutput_items __attribute__((unused)), + gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items __attribute__((unused)), + gr_vector_void_star &output_items); + private: gnss_sdr_time_counter(); int64_t current_T_rx_ms; // Receiver time in ms since the beginning of the run @@ -57,11 +64,6 @@ private: uint32_t current_days; // Receiver time in days since the beginning of the run int32_t report_interval_ms; friend gnss_sdr_time_counter_sptr gnss_sdr_make_time_counter(); - -public: - ~gnss_sdr_time_counter(); - int general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items __attribute__((unused)), gr_vector_void_star &output_items); }; #endif /*GNSS_SDR_GNSS_SDR_SAMPLE_COUNTER_H_*/ diff --git a/src/core/monitor/gnss_synchro_monitor.h b/src/core/monitor/gnss_synchro_monitor.h index 27f23e7c1..7cb104154 100644 --- a/src/core/monitor/gnss_synchro_monitor.h +++ b/src/core/monitor/gnss_synchro_monitor.h @@ -60,6 +60,12 @@ gnss_synchro_monitor_sptr gnss_synchro_make_monitor(unsigned int n_channels, */ class gnss_synchro_monitor : public gr::sync_block { +public: + ~gnss_synchro_monitor(); //!< Default destructor + + int work(int noutput_items, gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items); + private: friend gnss_synchro_monitor_sptr gnss_synchro_make_monitor(unsigned int n_channels, int decimation_factor, @@ -77,12 +83,6 @@ private: int d_decimation_factor; std::unique_ptr udp_sink_ptr; int count; - -public: - ~gnss_synchro_monitor(); //!< Default destructor - - int work(int noutput_items, gr_vector_const_void_star& input_items, - gr_vector_void_star& output_items); }; #endif diff --git a/src/core/receiver/concurrent_map.h b/src/core/receiver/concurrent_map.h index af3823bd1..f3d20f181 100644 --- a/src/core/receiver/concurrent_map.h +++ b/src/core/receiver/concurrent_map.h @@ -46,10 +46,6 @@ template class Concurrent_Map { typedef typename std::map::iterator Data_iterator; // iterator is scope dependent -private: - std::map the_map; - boost::mutex the_mutex; - public: void write(int key, Data const& data) { @@ -97,6 +93,10 @@ public: lock.unlock(); return false; } + +private: + std::map the_map; + boost::mutex the_mutex; }; #endif diff --git a/src/core/receiver/concurrent_queue.h b/src/core/receiver/concurrent_queue.h index 02c6dd896..66cefbf96 100644 --- a/src/core/receiver/concurrent_queue.h +++ b/src/core/receiver/concurrent_queue.h @@ -45,11 +45,6 @@ template */ class Concurrent_Queue { -private: - std::queue the_queue; - mutable boost::mutex the_mutex; - boost::condition_variable the_condition_variable; - public: void push(Data const& data) { @@ -87,5 +82,10 @@ public: popped_value = the_queue.front(); the_queue.pop(); } + +private: + std::queue the_queue; + mutable boost::mutex the_mutex; + boost::condition_variable the_condition_variable; }; #endif diff --git a/src/core/system_parameters/beidou_dnav_ephemeris.h b/src/core/system_parameters/beidou_dnav_ephemeris.h index 2cdfcfe61..903804961 100644 --- a/src/core/system_parameters/beidou_dnav_ephemeris.h +++ b/src/core/system_parameters/beidou_dnav_ephemeris.h @@ -44,18 +44,13 @@ */ class Beidou_Dnav_Ephemeris { -private: - /* - * Accounts for the beginning or end of week crossover - * - * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) - * \param[in] - time in seconds - * \param[out] - corrected time, in seconds - */ - double check_t(double time); - public: - unsigned int i_satellite_PRN; // SV PRN NUMBER + /*! + * Default constructor + */ + Beidou_Dnav_Ephemeris(); + + unsigned int i_satellite_PRN; //!< SV PRN NUMBER double d_TOW; //!< Time of BEIDOU Week of the ephemeris set (taken from subframes TOW) [s] double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] double d_Delta_n; //!< Mean Motion Difference From Computed Value [semi-circles/s] @@ -124,6 +119,25 @@ public: std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus + /*! + * \brief Compute the ECEF SV coordinates and ECEF velocity + * Implementation of Table 20-IV (IS-GPS-200E) + * and compute the clock bias term including relativistic effect (return value) + */ + double satellitePosition(double transmitTime); + + /*! + * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_drift(double transmitTime); + + /*! + * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_relativistic_term(double transmitTime); + template /*! @@ -177,30 +191,15 @@ public: archive& make_nvp("b_antispoofing_flag", b_antispoofing_flag); //!< If true, the AntiSpoofing mode is ON in that SV } - /*! - * \brief Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Table 20-IV (IS-GPS-200E) - * and compute the clock bias term including relativistic effect (return value) +private: + /* + * Accounts for the beginning or end of week crossover + * + * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) + * \param[in] - time in seconds + * \param[out] - corrected time, in seconds */ - double satellitePosition(double transmitTime); - - /*! - * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_drift(double transmitTime); - - /*! - * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_relativistic_term(double transmitTime); - - - /*! - * Default constructor - */ - Beidou_Dnav_Ephemeris(); + double check_t(double time); }; #endif diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.h b/src/core/system_parameters/beidou_dnav_navigation_message.h index 3d1a2a61e..7ec7d8375 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.h +++ b/src/core/system_parameters/beidou_dnav_navigation_message.h @@ -54,19 +54,6 @@ */ class Beidou_Dnav_Navigation_Message { -private: - uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); - int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); - bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); - void print_beidou_word_bytes(uint32_t BEIDOU_word); - /* - * Accounts for the beginning or end of week crossover - * - * \param[in] - time in seconds - * \param[out] - corrected time, in seconds - */ - double check_t(double time); - public: /*! * Default constructor @@ -207,7 +194,7 @@ public: // satellite identification info int32_t i_channel_ID; - int32_t i_signal_type; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) + int32_t i_signal_type; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) uint32_t i_satellite_PRN; // time synchro @@ -321,6 +308,20 @@ public: * \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed */ bool have_new_almanac(); + +private: + uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); + int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); + bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); + void print_beidou_word_bytes(uint32_t BEIDOU_word); + + /* + * Accounts for the beginning or end of week crossover + * + * \param[in] - time in seconds + * \param[out] - corrected time, in seconds + */ + double check_t(double time); }; #endif diff --git a/src/core/system_parameters/galileo_navigation_message.h b/src/core/system_parameters/galileo_navigation_message.h index 6d8b0363a..275cdf120 100644 --- a/src/core/system_parameters/galileo_navigation_message.h +++ b/src/core/system_parameters/galileo_navigation_message.h @@ -52,14 +52,9 @@ */ class Galileo_Navigation_Message { -private: - bool CRC_test(std::bitset bits, uint32_t checksum); - bool read_navigation_bool(std::bitset bits, const std::vector >& parameter); - uint64_t read_navigation_unsigned(std::bitset bits, const std::vector >& parameter); - uint64_t read_page_type_unsigned(std::bitset bits, const std::vector >& parameter); - int64_t read_navigation_signed(std::bitset bits, const std::vector >& parameter); - public: + Galileo_Navigation_Message(); + int32_t Page_type_time_stamp; int32_t flag_even_word; std::string page_Even; @@ -293,7 +288,12 @@ public: */ Galileo_Almanac_Helper get_almanac(); - Galileo_Navigation_Message(); +private: + bool CRC_test(std::bitset bits, uint32_t checksum); + bool read_navigation_bool(std::bitset bits, const std::vector >& parameter); + uint64_t read_navigation_unsigned(std::bitset bits, const std::vector >& parameter); + uint64_t read_page_type_unsigned(std::bitset bits, const std::vector >& parameter); + int64_t read_navigation_signed(std::bitset bits, const std::vector >& parameter); }; #endif /* GALILEO_NAVIGATION_MESSAGE_H_ */ diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index b5cf60f66..efa1ffd7b 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -46,17 +46,12 @@ */ class Glonass_Gnav_Ephemeris { -private: - /* - * Accounts for the beginning or end of week crossover - * - * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) - * \param[in] - time in seconds - * \param[out] - corrected time, in seconds - */ - double check_t(double time); - public: + /*! + * Default constructor + */ + Glonass_Gnav_Ephemeris(); + double d_m; //!< String number within frame [dimensionless] double d_t_k; //!< GLONASS Time (UTC(SU) + 3 h) referenced to the beginning of the frame within the current day [s] double d_t_b; //!< Reference ephemeris relative time in GLONASS Time (UTC(SU) + 3 h). Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [s] @@ -100,6 +95,38 @@ public: double d_WN; //!< GLONASST IN GPST week number of the start of frame double d_tod; //!< Time of Day since ephemeris where decoded + /*! + * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_drift(double transmitTime, double timeCorrUTC); + + /*! + * \brief Computes the GLONASS System Time and returns a boost::posix_time::ptime object + * \ param offset_time Is the start of day offset to compute the time + */ + boost::posix_time::ptime compute_GLONASS_time(const double offset_time) const; + + /*! + * \brief Converts from GLONASST to UTC + * \details The function simply adjust for the 6 hrs offset between GLONASST and UTC + * \param[in] offset_time Is the start of day offset + * \param[in] glot2utc_corr Correction from GLONASST to UTC + * \returns UTC time as a boost::posix_time::ptime object + */ + boost::posix_time::ptime glot_to_utc(const double offset_time, const double glot2utc_corr) const; + + /*! + * \brief Converts from GLONASST to GPST + * \details Converts from GLONASST to GPST in time of week (TOW) and week number (WN) format + * \param[in] tod_offset Is the start of day offset + * \param[in] glot2utc_corr Correction from GLONASST to UTC + * \param[in] glot2gpst_corr Correction from GLONASST to GPST + * \param[out] WN Week Number, not in mod(1024) format + * \param[out] TOW Time of Week in seconds of week + */ + void glot_to_gpst(double tod_offset, double glot2utc_corr, double glot2gpst_corr, double* WN, double* TOW) const; + template /*! @@ -145,42 +172,15 @@ public: archive& make_nvp("d_l5th_n", d_l5th_n); //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] } - /*! - * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) +private: + /* + * Accounts for the beginning or end of week crossover + * + * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) + * \param[in] - time in seconds + * \param[out] - corrected time, in seconds */ - double sv_clock_drift(double transmitTime, double timeCorrUTC); - - /*! - * \brief Computes the GLONASS System Time and returns a boost::posix_time::ptime object - * \ param offset_time Is the start of day offset to compute the time - */ - boost::posix_time::ptime compute_GLONASS_time(const double offset_time) const; - - /*! - * \brief Converts from GLONASST to UTC - * \details The function simply adjust for the 6 hrs offset between GLONASST and UTC - * \param[in] offset_time Is the start of day offset - * \param[in] glot2utc_corr Correction from GLONASST to UTC - * \returns UTC time as a boost::posix_time::ptime object - */ - boost::posix_time::ptime glot_to_utc(const double offset_time, const double glot2utc_corr) const; - - /*! - * \brief Converts from GLONASST to GPST - * \details Converts from GLONASST to GPST in time of week (TOW) and week number (WN) format - * \param[in] tod_offset Is the start of day offset - * \param[in] glot2utc_corr Correction from GLONASST to UTC - * \param[in] glot2gpst_corr Correction from GLONASST to GPST - * \param[out] WN Week Number, not in mod(1024) format - * \param[out] TOW Time of Week in seconds of week - */ - void glot_to_gpst(double tod_offset, double glot2utc_corr, double glot2gpst_corr, double* WN, double* TOW) const; - - /*! - * Default constructor - */ - Glonass_Gnav_Ephemeris(); + double check_t(double time); }; #endif diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 3c2af1243..e53a365e3 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -41,9 +41,9 @@ #include "glonass_gnav_utc_model.h" #include #include -#include // for pair -#include // for vector #include +#include // for pair +#include // for vector /*! @@ -53,12 +53,12 @@ */ class Glonass_Gnav_Navigation_Message { -private: - uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); - int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); - bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); - public: + /*! + * Default constructor + */ + Glonass_Gnav_Navigation_Message(); + bool flag_CRC_test; uint32_t d_frame_ID; uint32_t d_string_ID; @@ -164,10 +164,10 @@ public: */ int32_t string_decoder(const std::string& frame_string); - /*! - * Default constructor - */ - Glonass_Gnav_Navigation_Message(); +private: + uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); + int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); + bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); }; #endif diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index 5a082cb7e..04b6bb298 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -45,6 +45,11 @@ class Glonass_Gnav_Utc_Model { public: + /*! + * Default constructor + */ + Glonass_Gnav_Utc_Model(); + bool valid; // Clock Parameters double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] @@ -54,6 +59,12 @@ public: double d_B1; //!< Coefficient to determine DeltaUT1 [s] double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd] + /*! + * \brief Computes the Coordinated Universal Time (UTC) and + * returns it in [s] (GLONASS ICD (Edition 5.1) Section 3.3.3 GLONASS Time) + */ + double utc_time(double glonass_time_corrected); + template /*! * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the almanac data on disk file. @@ -72,17 +83,6 @@ public: archive& make_nvp("d_B1", d_B1); archive& make_nvp("d_B2", d_B2); } - - /*! - * Default constructor - */ - Glonass_Gnav_Utc_Model(); - - /*! - * \brief Computes the Coordinated Universal Time (UTC) and - * returns it in [s] (GLONASS ICD (Edition 5.1) Section 3.3.3 GLONASS Time) - */ - double utc_time(double glonass_time_corrected); }; #endif diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index a2abc773f..c2f6e37c4 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -43,10 +43,6 @@ */ class Gnss_Signal { -private: - Gnss_Satellite satellite; - std::string signal; - public: Gnss_Signal(); Gnss_Signal(const std::string& signal_); @@ -57,6 +53,10 @@ public: friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing + +private: + Gnss_Satellite satellite; + std::string signal; }; #endif diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index aed92612b..2450ed004 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -43,10 +43,12 @@ */ class Gps_CNAV_Ephemeris { -private: - double check_t(double time); - public: + /*! + * Default constructor + */ + Gps_CNAV_Ephemeris(); + uint32_t i_satellite_PRN; // SV PRN NUMBER // Message Types 10 and 11 Parameters (1 of 2) @@ -122,6 +124,24 @@ public: double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m] double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m] + /*! + * \brief Compute the ECEF SV coordinates and ECEF velocity + * Implementation of Table 20-IV (IS-GPS-200E) + */ + double satellitePosition(double transmitTime); + + /*! + * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_drift(double transmitTime); + + /*! + * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_relativistic_term(double transmitTime); + template /*! @@ -170,27 +190,8 @@ public: archive& make_nvp("b_antispoofing_flag", b_antispoofing_flag); //!< If true, the AntiSpoofing mode is ON in that SV } - /*! - * \brief Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Table 20-IV (IS-GPS-200E) - */ - double satellitePosition(double transmitTime); - - /*! - * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_drift(double transmitTime); - - /*! - * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_relativistic_term(double transmitTime); - /*! - * Default constructor - */ - Gps_CNAV_Ephemeris(); +private: + double check_t(double time); }; #endif diff --git a/src/core/system_parameters/gps_cnav_navigation_message.h b/src/core/system_parameters/gps_cnav_navigation_message.h index 62fc6fbc1..d29bbed2c 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.h +++ b/src/core/system_parameters/gps_cnav_navigation_message.h @@ -55,16 +55,12 @@ */ class Gps_CNAV_Navigation_Message { -private: - uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); - int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); - bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); - - Gps_CNAV_Ephemeris ephemeris_record; - Gps_CNAV_Iono iono_record; - Gps_CNAV_Utc_Model utc_model_record; - public: + /*! + * Default constructor + */ + Gps_CNAV_Navigation_Message(); + int32_t d_TOW; bool b_flag_ephemeris_1; bool b_flag_ephemeris_2; @@ -122,10 +118,14 @@ public: */ bool have_new_ephemeris(); - /*! - * Default constructor - */ - Gps_CNAV_Navigation_Message(); +private: + uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); + int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); + bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); + + Gps_CNAV_Ephemeris ephemeris_record; + Gps_CNAV_Iono iono_record; + Gps_CNAV_Utc_Model utc_model_record; }; #endif diff --git a/src/core/system_parameters/gps_ephemeris.h b/src/core/system_parameters/gps_ephemeris.h index e78b34207..c54d3bcab 100644 --- a/src/core/system_parameters/gps_ephemeris.h +++ b/src/core/system_parameters/gps_ephemeris.h @@ -46,17 +46,12 @@ */ class Gps_Ephemeris { -private: - /* - * Accounts for the beginning or end of week crossover - * - * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) - * \param[in] - time in seconds - * \param[out] - corrected time, in seconds - */ - double check_t(double time); - public: + /*! + * Default constructor + */ + Gps_Ephemeris(); + uint32_t i_satellite_PRN; // SV PRN NUMBER int32_t d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] @@ -127,6 +122,25 @@ public: std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus + /*! + * \brief Compute the ECEF SV coordinates and ECEF velocity + * Implementation of Table 20-IV (IS-GPS-200E) + * and compute the clock bias term including relativistic effect (return value) + */ + double satellitePosition(double transmitTime); + + /*! + * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_drift(double transmitTime); + + /*! + * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_relativistic_term(double transmitTime); + template /*! @@ -182,29 +196,15 @@ public: archive& make_nvp("b_antispoofing_flag", b_antispoofing_flag); //!< If true, the AntiSpoofing mode is ON in that SV } - /*! - * \brief Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Table 20-IV (IS-GPS-200E) - * and compute the clock bias term including relativistic effect (return value) +private: + /* + * Accounts for the beginning or end of week crossover + * + * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) + * \param[in] - time in seconds + * \param[out] - corrected time, in seconds */ - double satellitePosition(double transmitTime); - - /*! - * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_drift(double transmitTime); - - /*! - * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_relativistic_term(double transmitTime); - - /*! - * Default constructor - */ - Gps_Ephemeris(); + double check_t(double time); }; #endif diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index 21a4aa4cd..7f8f68543 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -52,13 +52,12 @@ */ class Gps_Navigation_Message { -private: - uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); - int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); - bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); - void print_gps_word_bytes(uint32_t GPS_word); - public: + /*! + * Default constructor + */ + Gps_Navigation_Message(); + bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check // broadcast orbit 1 int32_t d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] @@ -208,10 +207,11 @@ public: bool satellite_validation(); - /*! - * Default constructor - */ - Gps_Navigation_Message(); +private: + uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter); + int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter); + bool read_navigation_bool(std::bitset bits, const std::vector>& parameter); + void print_gps_word_bytes(uint32_t GPS_word); }; #endif diff --git a/src/core/system_parameters/sbas_ephemeris.h b/src/core/system_parameters/sbas_ephemeris.h index 61749b482..81acfdebd 100644 --- a/src/core/system_parameters/sbas_ephemeris.h +++ b/src/core/system_parameters/sbas_ephemeris.h @@ -42,18 +42,16 @@ class Sbas_Ephemeris { public: void print(std::ostream &out); - int i_prn; //!< PRN number - //gtime_t t0; // reference epoch time (GPST) - int i_t0; - //gtime_t tof; // time of message frame (GPST) - double d_tof; - int i_sv_ura; //!< SV accuracy (URA index), not standardized - bool b_sv_do_not_use; //!< Health status (false:do not use / true:usable) - double d_pos[3]; //!< Satellite position (m) (ECEF) - double d_vel[3]; //!< Satellite velocity (m/s) (ECEF) - double d_acc[3]; //!< Satellite acceleration (m/s^2) (ECEF) - double d_af0; //!< Satellite clock-offset (s) - double d_af1; //!< Satellite drift (s/s) + int i_prn; //!< PRN number + int i_t0; //!< Reference epoch time (GPST) + double d_tof; //!< Time of message frame (GPST) + int i_sv_ura; //!< SV accuracy (URA index), not standardized + bool b_sv_do_not_use; //!< Health status (false:do not use / true:usable) + double d_pos[3]; //!< Satellite position (m) (ECEF) + double d_vel[3]; //!< Satellite velocity (m/s) (ECEF) + double d_acc[3]; //!< Satellite acceleration (m/s^2) (ECEF) + double d_af0; //!< Satellite clock-offset (s) + double d_af1; //!< Satellite drift (s/s) }; From f0a92f1fb32164c5d6ee59a93fedb7b0e0cbad27 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 2 Jul 2019 06:54:48 +0200 Subject: [PATCH 32/64] Make constructor public --- src/algorithms/signal_source/libs/gnss_sdr_valve.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/algorithms/signal_source/libs/gnss_sdr_valve.h b/src/algorithms/signal_source/libs/gnss_sdr_valve.h index 0ab0d08ab..9bd90a188 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_valve.h +++ b/src/algorithms/signal_source/libs/gnss_sdr_valve.h @@ -65,6 +65,10 @@ public: gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + Gnss_Sdr_Valve(size_t sizeof_stream_item, + uint64_t nitems, + gr::msg_queue::sptr queue, bool stop_flowgraph); + private: friend boost::shared_ptr gnss_sdr_make_valve( size_t sizeof_stream_item, @@ -77,10 +81,6 @@ private: gr::msg_queue::sptr queue, bool stop_flowgraph); - Gnss_Sdr_Valve(size_t sizeof_stream_item, - uint64_t nitems, - gr::msg_queue::sptr queue, bool stop_flowgraph); - uint64_t d_nitems; uint64_t d_ncopied_items; gr::msg_queue::sptr d_queue; From dfecb45b7f8ae3e368ee903fc2c054ff58f9b6fe Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 2 Jul 2019 11:26:59 +0200 Subject: [PATCH 33/64] Fix warning (unsigned comparison) --- src/core/libs/gnss_sdr_supl_client.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index 1729583cd..ae7d60db0 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -174,7 +174,7 @@ int Gnss_Sdr_Supl_Client::get_assistance(int i_mcc, int i_mns, int i_lac, int i_ // PERFORM SUPL COMMUNICATION std::vector cstr(server_name.length() + 1); - for (int i = 0; i != server_name.length(); ++i) + for (unsigned int i = 0; i != server_name.length(); ++i) { cstr[i] = static_cast(server_name[i]); } From 173361f89f782a31afcd9d8e624e6426e058140f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 2 Jul 2019 11:31:43 +0200 Subject: [PATCH 34/64] Fix test --- .../tracking/gpu_multicorrelator_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc index 5048fe638..b16b5b693 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc @@ -96,7 +96,7 @@ TEST(GpuMulticorrelatorTest, MeasureExecutionTime) //--- Perform initializations ------------------------------ //local code resampler on GPU // generate local reference (1 sample per chip) - gps_l1_ca_code_gen_complex(d_ca_code, 1, 0); + gps_l1_ca_code_gen_complex(gsl::span(d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS)), 1, 0); // generate inut signal for (int n = 0; n < 2 * d_vector_length; n++) { From da7ca482a8030b60615ae59487d74f967cb259f4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 2 Jul 2019 17:24:54 +0200 Subject: [PATCH 35/64] Hide direct constructor as private member --- .../input_filter/adapters/beamformer_filter.cc | 2 +- .../input_filter/gnuradio_blocks/beamformer.cc | 2 +- .../input_filter/gnuradio_blocks/beamformer.h | 4 ++-- .../input_filter/gnuradio_blocks/notch_cc.h | 12 ++++++++---- .../gnuradio_blocks/notch_lite_cc.h | 12 +++++++++--- .../gnuradio_blocks/pulse_blanking_cc.h | 11 +++++++---- .../signal_source/libs/gnss_sdr_valve.cc | 4 ++-- .../signal_source/libs/gnss_sdr_valve.h | 18 ++++++++++-------- .../tracking/tracking_pull-in_test.cc | 3 +-- 9 files changed, 41 insertions(+), 27 deletions(-) diff --git a/src/algorithms/input_filter/adapters/beamformer_filter.cc b/src/algorithms/input_filter/adapters/beamformer_filter.cc index 2975b8d1d..52f46ca6b 100644 --- a/src/algorithms/input_filter/adapters/beamformer_filter.cc +++ b/src/algorithms/input_filter/adapters/beamformer_filter.cc @@ -49,7 +49,7 @@ BeamformerFilter::BeamformerFilter( if (item_type_ == "gr_complex") { item_size_ = sizeof(gr_complex); - beamformer_ = make_beamformer(); + beamformer_ = make_beamformer_sptr(); DLOG(INFO) << "Item size " << item_size_; DLOG(INFO) << "resampler(" << beamformer_->unique_id() << ")"; } diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc index 6f53c6099..9656f57d0 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc @@ -36,7 +36,7 @@ #define GNSS_SDR_BEAMFORMER_CHANNELS 8 -beamformer_sptr make_beamformer() +beamformer_sptr make_beamformer_sptr() { return beamformer_sptr(new beamformer()); } diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h index 55e9fcc27..b9cdd247a 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h @@ -36,7 +36,7 @@ class beamformer; using beamformer_sptr = boost::shared_ptr; -beamformer_sptr make_beamformer(); +beamformer_sptr make_beamformer_sptr(); /*! * \brief This class implements a real-time software-defined spatial filter using the CTTC GNSS experimental antenna array input and a set of dynamically reloadable weights @@ -44,13 +44,13 @@ beamformer_sptr make_beamformer(); class beamformer : public gr::sync_block { public: - beamformer(); ~beamformer(); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); private: friend beamformer_sptr make_beamformer_sptr(); + beamformer(); gr_complex *weight_vector; }; diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h index 4f898b154..4640ba4d1 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h @@ -41,8 +41,12 @@ class Notch; using notch_sptr = boost::shared_ptr; -notch_sptr make_notch_filter(float pfa, float p_c_factor, - int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); +notch_sptr make_notch_filter( + float pfa, + float p_c_factor, + int32_t length_, + int32_t n_segments_est, + int32_t n_segments_reset); /*! * \brief This class implements a real-time software-defined multi state notch filter @@ -50,8 +54,6 @@ notch_sptr make_notch_filter(float pfa, float p_c_factor, class Notch : public gr::block { public: - Notch(float pfa, float p_c_factor, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); - ~Notch(); void forecast(int noutput_items, gr_vector_int &ninput_items_required); @@ -61,6 +63,8 @@ public: gr_vector_void_star &output_items); private: + friend notch_sptr make_notch_filter(float pfa, float p_c_factor, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); + Notch(float pfa, float p_c_factor, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); float pfa; float noise_pow_est; float thres_; diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h index 0a7114240..7b274a13c 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h @@ -41,7 +41,13 @@ class NotchLite; using notch_lite_sptr = boost::shared_ptr; -notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset, int32_t n_segments_coeff); +notch_lite_sptr make_notch_filter_lite( + float p_c_factor, + float pfa, + int32_t length_, + int32_t n_segments_est, + int32_t n_segments_reset, + int32_t n_segments_coeff); /*! * \brief This class implements a real-time software-defined multi state notch filter light version @@ -49,8 +55,6 @@ notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int32_t leng class NotchLite : public gr::block { public: - NotchLite(float p_c_factor, float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset, int32_t n_segments_coeff); - ~NotchLite(); void forecast(int noutput_items, gr_vector_int &ninput_items_required); @@ -60,6 +64,8 @@ public: gr_vector_void_star &output_items); private: + friend notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset, int32_t n_segments_coeff); + NotchLite(float p_c_factor, float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset, int32_t n_segments_coeff); int32_t length_; int32_t n_segments; int32_t n_segments_est; diff --git a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h index 52abef24f..edbd9c5e8 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h @@ -39,14 +39,15 @@ class pulse_blanking_cc; using pulse_blanking_cc_sptr = boost::shared_ptr; -pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); - +pulse_blanking_cc_sptr make_pulse_blanking_cc( + float pfa, + int32_t length_, + int32_t n_segments_est, + int32_t n_segments_reset); class pulse_blanking_cc : public gr::block { public: - pulse_blanking_cc(float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); - ~pulse_blanking_cc(); void forecast(int noutput_items, gr_vector_int &ninput_items_required); @@ -55,6 +56,8 @@ public: gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); private: + friend pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); + pulse_blanking_cc(float pfa, int32_t length_, int32_t n_segments_est, int32_t n_segments_reset); int32_t length_; int32_t n_segments; int32_t n_segments_est; diff --git a/src/algorithms/signal_source/libs/gnss_sdr_valve.cc b/src/algorithms/signal_source/libs/gnss_sdr_valve.cc index aa2bd1237..615cb65b7 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_valve.cc +++ b/src/algorithms/signal_source/libs/gnss_sdr_valve.cc @@ -55,14 +55,14 @@ Gnss_Sdr_Valve::Gnss_Sdr_Valve(size_t sizeof_stream_item, } -boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue, bool stop_flowgraph) +boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue, bool stop_flowgraph) { boost::shared_ptr valve_(new Gnss_Sdr_Valve(sizeof_stream_item, nitems, std::move(queue), stop_flowgraph)); return valve_; } -boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue) +boost::shared_ptr gnss_sdr_make_valve(size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue) { boost::shared_ptr valve_(new Gnss_Sdr_Valve(sizeof_stream_item, nitems, std::move(queue), true)); return valve_; diff --git a/src/algorithms/signal_source/libs/gnss_sdr_valve.h b/src/algorithms/signal_source/libs/gnss_sdr_valve.h index 9bd90a188..b56807c2b 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_valve.h +++ b/src/algorithms/signal_source/libs/gnss_sdr_valve.h @@ -41,12 +41,14 @@ #include // for size_t #include -boost::shared_ptr gnss_sdr_make_valve( +class Gnss_Sdr_Valve; + +boost::shared_ptr gnss_sdr_make_valve( size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue); -boost::shared_ptr gnss_sdr_make_valve( +boost::shared_ptr gnss_sdr_make_valve( size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue, @@ -65,22 +67,22 @@ public: gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - Gnss_Sdr_Valve(size_t sizeof_stream_item, - uint64_t nitems, - gr::msg_queue::sptr queue, bool stop_flowgraph); - private: - friend boost::shared_ptr gnss_sdr_make_valve( + friend boost::shared_ptr gnss_sdr_make_valve( size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue); - friend boost::shared_ptr gnss_sdr_make_valve( + friend boost::shared_ptr gnss_sdr_make_valve( size_t sizeof_stream_item, uint64_t nitems, gr::msg_queue::sptr queue, bool stop_flowgraph); + Gnss_Sdr_Valve(size_t sizeof_stream_item, + uint64_t nitems, + gr::msg_queue::sptr queue, bool stop_flowgraph); + uint64_t d_nitems; uint64_t d_ncopied_items; gr::msg_queue::sptr d_queue; diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc index e7aa0676f..fb38b8efa 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc @@ -796,9 +796,8 @@ TEST_F(TrackingPullInTest, ValidationOfResults) // create the msg queue for valve queue = gr::msg_queue::make(0); - boost::shared_ptr reseteable_valve; long long int acq_to_trk_delay_samples = ceil(static_cast(FLAGS_fs_gen_sps) * FLAGS_acq_to_trk_delay_s); - boost::shared_ptr resetable_valve_(new Gnss_Sdr_Valve(sizeof(gr_complex), acq_to_trk_delay_samples, queue, false)); + auto resetable_valve_ = gnss_sdr_make_valve(sizeof(gr_complex), acq_to_trk_delay_samples, queue, false); std::shared_ptr control_message_factory_; std::shared_ptr>> control_messages_; From 081439d6f830f4b05a6cd5796829d44f2af3a229 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 2 Jul 2019 17:48:00 +0200 Subject: [PATCH 36/64] Code cleaning --- .../acquisition/libs/fpga_acquisition.cc | 4 ++-- .../acquisition/libs/fpga_acquisition.h | 20 +++++++++++++++---- src/algorithms/channel/libs/channel_fsm.cc | 5 +++++ .../channel/libs/channel_msg_receiver_cc.h | 4 ++-- 4 files changed, 25 insertions(+), 8 deletions(-) diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index 00b1b65b3..16d5534de 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -81,7 +81,6 @@ Fpga_Acquisition::Fpga_Acquisition(std::string device_name, int64_t fs_in, uint32_t sampled_ms __attribute__((unused)), uint32_t select_queue, - //lv_16sc_t *all_fft_codes, uint32_t *all_fft_codes, uint32_t excludelimit) { @@ -209,6 +208,7 @@ void Fpga_Acquisition::set_block_exp(uint32_t total_block_exp) d_map_base[11] = total_block_exp; } + void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_step, int32_t doppler_min) { float phase_step_rad_real; @@ -233,7 +233,6 @@ void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_s void Fpga_Acquisition::configure_acquisition() { //Fpga_Acquisition::open_device(); - d_map_base[0] = d_select_queue; d_map_base[1] = d_vector_length; d_map_base[2] = d_nsamples; @@ -318,6 +317,7 @@ void Fpga_Acquisition::read_fpga_total_scale_factor(uint32_t *total_scale_factor *fw_scale_factor = 0; } + void Fpga_Acquisition::read_result_valid(uint32_t *result_valid) { uint32_t readval = 0; diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.h b/src/algorithms/acquisition/libs/fpga_acquisition.h index 805297bd3..823400290 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.h +++ b/src/algorithms/acquisition/libs/fpga_acquisition.h @@ -36,7 +36,6 @@ #ifndef GNSS_SDR_FPGA_ACQUISITION_H_ #define GNSS_SDR_FPGA_ACQUISITION_H_ -#include // for lv_16sc_t #include #include @@ -46,7 +45,8 @@ class Fpga_Acquisition { public: - Fpga_Acquisition(std::string device_name, + Fpga_Acquisition( + std::string device_name, uint32_t nsamples, uint32_t doppler_max, uint32_t nsamples_total, @@ -57,13 +57,24 @@ public: uint32_t excludelimit); ~Fpga_Acquisition(); + bool set_local_code(uint32_t PRN); + void set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_step, int32_t doppler_min); + void run_acquisition(void); - void read_acquisition_results(uint32_t *max_index, float *firstpeak, float *secondpeak, uint64_t *initial_sample, float *power_sum, uint32_t *doppler_index, uint32_t *total_blk_exp); + void read_acquisition_results( + uint32_t *max_index, + float *firstpeak, + float *secondpeak, + uint64_t *initial_sample, + float *power_sum, + uint32_t *doppler_index, + uint32_t *total_blk_exp); void block_samples(); + void unblock_samples(); /*! @@ -100,9 +111,10 @@ public: void configure_acquisition(void); - void close_device(); void open_device(); + void close_device(); + private: int64_t d_fs_in; // data related to the hardware module and the driver diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index f5e75ed7c..ac533d310 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -87,6 +87,7 @@ bool ChannelFsm::Event_start_acquisition_fpga() return true; } + bool ChannelFsm::Event_start_acquisition() { std::lock_guard lk(mx); @@ -170,11 +171,14 @@ void ChannelFsm::set_tracking(std::shared_ptr tracking) trk_ = std::move(tracking); } + void ChannelFsm::set_telemetry(std::shared_ptr telemetry) { std::lock_guard lk(mx); nav_ = std::move(telemetry); } + + void ChannelFsm::set_queue(gr::msg_queue::sptr queue) { std::lock_guard lk(mx); @@ -194,6 +198,7 @@ void ChannelFsm::stop_acquisition() acq_->stop_acquisition(); } + void ChannelFsm::stop_tracking() { trk_->stop_tracking(); diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.h b/src/algorithms/channel/libs/channel_msg_receiver_cc.h index 7f305f7f7..8b38e10ab 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.h +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.h @@ -51,11 +51,11 @@ public: ~channel_msg_receiver_cc(); //!< Default destructor private: + friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); + channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat); std::shared_ptr d_channel_fsm; bool d_repeat; // todo: change FSM to include repeat value - friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); void msg_handler_events(pmt::pmt_t msg); - channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat); }; #endif From cf967be25277a45f246b02b02ed5277cf5f0c2dc Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 3 Jul 2019 18:57:26 +0200 Subject: [PATCH 37/64] Consolidating unified dll pll veml tracking to synchronize symbols for GPS L1, L5, Galileo E1, E5 and Beidou B1, B3. Beidou bug fixes in acquisition. Adapting all telemetry decoders to use 1 sample per symbol --- .../adapters/beidou_b1i_pcps_acquisition.cc | 89 ++--- .../adapters/beidou_b1i_pcps_acquisition.h | 20 +- .../adapters/beidou_b3i_pcps_acquisition.cc | 101 +++-- .../adapters/beidou_b3i_pcps_acquisition.h | 4 +- src/algorithms/libs/gnss_sdr_flags.cc | 2 +- .../beidou_b1i_telemetry_decoder_gs.cc | 226 +++++++---- .../beidou_b1i_telemetry_decoder_gs.h | 14 +- .../beidou_b3i_telemetry_decoder_gs.cc | 378 +++++++++--------- .../beidou_b3i_telemetry_decoder_gs.h | 21 +- .../galileo_telemetry_decoder_gs.cc | 8 +- .../gps_l1_ca_telemetry_decoder_gs.cc | 14 +- .../adapters/beidou_b3i_dll_pll_tracking.cc | 48 ++- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 64 ++- src/algorithms/tracking/libs/dll_pll_conf.cc | 1 + src/algorithms/tracking/libs/dll_pll_conf.h | 1 + src/core/system_parameters/Beidou_B1I.h | 1 + src/core/system_parameters/Beidou_B3I.h | 5 +- 17 files changed, 543 insertions(+), 454 deletions(-) diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc index 1c266882b..27dad02cd 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc @@ -51,48 +51,37 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( in_streams_(in_streams), out_streams_(out_streams) { - Acq_Conf acq_parameters = Acq_Conf(); configuration_ = configuration; std::string default_item_type = "gr_complex"; - std::string default_dump_filename = "./data/acquisition.dat"; + std::string default_dump_filename = "./acquisition.mat"; - DLOG(INFO) << "role " << role; + LOG(INFO) << "role " << role; item_type_ = configuration_->property(role + ".item_type", default_item_type); + int64_t fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - acq_parameters.fs_in = fs_in_; + acq_parameters_.fs_in = fs_in_; dump_ = configuration_->property(role + ".dump", false); - acq_parameters.dump = dump_; + acq_parameters_.dump = dump_; + acq_parameters_.dump_channel = configuration_->property(role + ".dump_channel", 0); blocking_ = configuration_->property(role + ".blocking", true); - acq_parameters.blocking = blocking_; - doppler_max_ = configuration_->property(role + ".doppler_max", 5000); + acq_parameters_.blocking = blocking_; + doppler_max_ = configuration->property(role + ".doppler_max", 5000); if (FLAGS_doppler_max != 0) { doppler_max_ = FLAGS_doppler_max; } - acq_parameters.doppler_max = doppler_max_; - sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); - acq_parameters.sampled_ms = sampled_ms_; + acq_parameters_.doppler_max = doppler_max_; bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); - acq_parameters.bit_transition_flag = bit_transition_flag_; + acq_parameters_.bit_transition_flag = bit_transition_flag_; use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions - acq_parameters.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; + acq_parameters_.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; max_dwells_ = configuration_->property(role + ".max_dwells", 1); - acq_parameters.max_dwells = max_dwells_; + acq_parameters_.max_dwells = max_dwells_; dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); - acq_parameters.dump_filename = dump_filename_; - //--- Find number of samples per spreading code ------------------------- - code_length_ = static_cast(std::round(static_cast(fs_in_) / (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS))); - - vector_length_ = code_length_ * sampled_ms_; - - if (bit_transition_flag_) - { - vector_length_ *= 2; - } - - code_ = std::vector>(vector_length_); + acq_parameters_.dump_filename = dump_filename_; + acq_parameters_.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); if (item_type_ == "cshort") { @@ -102,18 +91,28 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( { item_size_ = sizeof(gr_complex); } - acq_parameters.it_size = item_size_; - acq_parameters.sampled_ms = sampled_ms_; - acq_parameters.samples_per_ms = code_length_; - acq_parameters.samples_per_code = code_length_; - acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); - acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); - acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); - acquisition_ = pcps_make_acquisition(acq_parameters); - DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; - stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); - DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; + acq_parameters_.ms_per_code = 1; + acq_parameters_.it_size = item_size_; + num_codes_ = acq_parameters_.sampled_ms; + acq_parameters_.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters_.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters_.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters_.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false); + acq_parameters_.use_automatic_resampler = configuration_->property("GNSS-SDR.use_acquisition_resampler", false); + + acq_parameters_.resampled_fs = fs_in_; + //--- Find number of samples per spreading code ------------------------- + code_length_ = static_cast(std::floor(static_cast(fs_in_) / (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS))); + acq_parameters_.samples_per_ms = static_cast(fs_in_) * 0.001; + acq_parameters_.samples_per_chip = static_cast(ceil((1.0 / BEIDOU_B1I_CODE_RATE_HZ) * static_cast(acq_parameters_.fs_in))); + + + acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(BEIDOU_B1I_CODE_PERIOD * 1000.0); + vector_length_ = std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2 : 1); + code_ = std::vector>(vector_length_); + acquisition_ = pcps_make_acquisition(acq_parameters_); + DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; if (item_type_ == "cbyte") { @@ -208,7 +207,7 @@ void BeidouB1iPcpsAcquisition::set_local_code() beidou_b1i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); gsl::span code_span(code_.data(), vector_length_); - for (unsigned int i = 0; i < sampled_ms_; i++) + for (unsigned int i = 0; i < num_codes_; i++) { std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } @@ -233,15 +232,7 @@ float BeidouB1iPcpsAcquisition::calculate_threshold(float pfa) { //Calculate the threshold uint32_t frequency_bins = 0; - /* - for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_) - { - frequency_bins++; - } - */ - frequency_bins = (2 * doppler_max_ + doppler_step_) / doppler_step_; - DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; uint32_t ncells = vector_length_ * frequency_bins; double exponent = 1 / static_cast(ncells); @@ -266,9 +257,11 @@ void BeidouB1iPcpsAcquisition::connect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { + // Since a byte-based acq implementation is not available, + // we just convert cshorts to gr_complex top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->connect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->connect(float_to_complex_, 0, acquisition_, 0); } else { @@ -289,11 +282,9 @@ void BeidouB1iPcpsAcquisition::disconnect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { - // Since a byte-based acq implementation is not available, - // we just convert cshorts to gr_complex top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->disconnect(float_to_complex_, 0, acquisition_, 0); } else { diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h index 503b6409e..0c2e611e2 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h @@ -163,22 +163,21 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; - gr::blocks::stream_to_vector::sptr stream_to_vector_; + Acq_Conf acq_parameters_; gr::blocks::float_to_complex::sptr float_to_complex_; complex_byte_to_float_x2_sptr cbyte_to_float_x2_; size_t item_size_; std::string item_type_; - uint32_t vector_length_; - uint32_t code_length_; + unsigned int vector_length_; + unsigned int code_length_; bool bit_transition_flag_; bool use_CFAR_algorithm_flag_; - uint32_t channel_; + unsigned int channel_; std::weak_ptr channel_fsm_; float threshold_; - uint32_t doppler_max_; - uint32_t doppler_step_; - uint32_t sampled_ms_; - uint32_t max_dwells_; + unsigned int doppler_max_; + unsigned int doppler_step_; + unsigned int max_dwells_; int64_t fs_in_; bool dump_; bool blocking_; @@ -186,8 +185,9 @@ private: std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; - uint32_t in_streams_; - uint32_t out_streams_; + unsigned int num_codes_; + unsigned int in_streams_; + unsigned int out_streams_; float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc index 3a51a2a77..bdeede24e 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc @@ -49,49 +49,37 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( in_streams_(in_streams), out_streams_(out_streams) { - Acq_Conf acq_parameters = Acq_Conf(); configuration_ = configuration; std::string default_item_type = "gr_complex"; - std::string default_dump_filename = "./data/acquisition.dat"; + std::string default_dump_filename = "./acquisition.mat"; - DLOG(INFO) << "role " << role; + LOG(INFO) << "role " << role; item_type_ = configuration_->property(role + ".item_type", default_item_type); int64_t fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - acq_parameters.fs_in = fs_in_; + acq_parameters_.fs_in = fs_in_; dump_ = configuration_->property(role + ".dump", false); - acq_parameters.dump = dump_; + acq_parameters_.dump = dump_; + acq_parameters_.dump_channel = configuration_->property(role + ".dump_channel", 0); blocking_ = configuration_->property(role + ".blocking", true); - acq_parameters.blocking = blocking_; - doppler_max_ = configuration_->property(role + ".doppler_max", 5000); + acq_parameters_.blocking = blocking_; + doppler_max_ = configuration->property(role + ".doppler_max", 5000); if (FLAGS_doppler_max != 0) { doppler_max_ = FLAGS_doppler_max; } - acq_parameters.doppler_max = doppler_max_; - sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); - acq_parameters.sampled_ms = sampled_ms_; + acq_parameters_.doppler_max = doppler_max_; bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); - acq_parameters.bit_transition_flag = bit_transition_flag_; + acq_parameters_.bit_transition_flag = bit_transition_flag_; use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions - acq_parameters.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; + acq_parameters_.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; max_dwells_ = configuration_->property(role + ".max_dwells", 1); - acq_parameters.max_dwells = max_dwells_; + acq_parameters_.max_dwells = max_dwells_; dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); - acq_parameters.dump_filename = dump_filename_; - //--- Find number of samples per spreading code ------------------------- - code_length_ = static_cast(std::round(static_cast(fs_in_) / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS))); - - vector_length_ = code_length_ * sampled_ms_; - - if (bit_transition_flag_) - { - vector_length_ *= 2; - } - - code_ = std::vector>(vector_length_); + acq_parameters_.dump_filename = dump_filename_; + acq_parameters_.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); if (item_type_ == "cshort") { @@ -101,18 +89,28 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( { item_size_ = sizeof(gr_complex); } - acq_parameters.it_size = item_size_; - acq_parameters.sampled_ms = sampled_ms_; - acq_parameters.samples_per_ms = code_length_; - acq_parameters.samples_per_code = code_length_; - acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); - acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); - acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); - acquisition_ = pcps_make_acquisition(acq_parameters); - DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; - stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); - DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; + acq_parameters_.ms_per_code = 1; + acq_parameters_.it_size = item_size_; + num_codes_ = acq_parameters_.sampled_ms; + acq_parameters_.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters_.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters_.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters_.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false); + acq_parameters_.use_automatic_resampler = configuration_->property("GNSS-SDR.use_acquisition_resampler", false); + + acq_parameters_.resampled_fs = fs_in_; + //--- Find number of samples per spreading code ------------------------- + code_length_ = static_cast(std::floor(static_cast(fs_in_) / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS))); + acq_parameters_.samples_per_ms = static_cast(fs_in_) * 0.001; + acq_parameters_.samples_per_chip = static_cast(ceil((1.0 / BEIDOU_B3I_CODE_RATE_HZ) * static_cast(acq_parameters_.fs_in))); + + + acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(BEIDOU_B3I_CODE_PERIOD * 1000.0); + vector_length_ = std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2 : 1); + code_ = std::vector>(vector_length_); + acquisition_ = pcps_make_acquisition(acq_parameters_); + DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; if (item_type_ == "cbyte") { @@ -146,8 +144,12 @@ void BeidouB3iPcpsAcquisition::stop_acquisition() void BeidouB3iPcpsAcquisition::set_threshold(float threshold) { - float pfa = configuration_->property(role_ + ".pfa", 0.0); + float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); + if (pfa == 0.0) + { + pfa = configuration_->property(role_ + ".pfa", 0.0); + } if (pfa == 0.0) { threshold_ = threshold; @@ -196,7 +198,6 @@ signed int BeidouB3iPcpsAcquisition::mag() void BeidouB3iPcpsAcquisition::init() { acquisition_->init(); - set_local_code(); } @@ -207,7 +208,7 @@ void BeidouB3iPcpsAcquisition::set_local_code() beidou_b3i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); gsl::span code_span(code_.data(), vector_length_); - for (unsigned int i = 0; i < sampled_ms_; i++) + for (unsigned int i = 0; i < num_codes_; i++) { std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } @@ -232,20 +233,12 @@ float BeidouB3iPcpsAcquisition::calculate_threshold(float pfa) { //Calculate the threshold unsigned int frequency_bins = 0; - /* - for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_) - { - frequency_bins++; - } - */ - frequency_bins = (2 * doppler_max_ + doppler_step_) / doppler_step_; - DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; unsigned int ncells = vector_length_ * frequency_bins; - double exponent = 1 / static_cast(ncells); + double exponent = 1.0 / static_cast(ncells); double val = pow(1.0 - pfa, exponent); - auto lambda = static_cast(vector_length_); + auto lambda = double(vector_length_); boost::math::exponential_distribution mydist(lambda); auto threshold = static_cast(quantile(mydist, val)); @@ -265,9 +258,11 @@ void BeidouB3iPcpsAcquisition::connect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { + // Since a byte-based acq implementation is not available, + // we just convert cshorts to gr_complex top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->connect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->connect(float_to_complex_, 0, acquisition_, 0); } else { @@ -288,11 +283,9 @@ void BeidouB3iPcpsAcquisition::disconnect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { - // Since a byte-based acq implementation is not available, - // we just convert cshorts to gr_complex top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->disconnect(float_to_complex_, 0, acquisition_, 0); } else { @@ -307,7 +300,7 @@ gr::basic_block_sptr BeidouB3iPcpsAcquisition::get_left_block() { return acquisition_; } - else if (item_type_ == "cshort") + if (item_type_ == "cshort") { return acquisition_; } diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h index db2c6fbaa..c57f28127 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h @@ -162,7 +162,7 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; - gr::blocks::stream_to_vector::sptr stream_to_vector_; + Acq_Conf acq_parameters_; gr::blocks::float_to_complex::sptr float_to_complex_; complex_byte_to_float_x2_sptr cbyte_to_float_x2_; size_t item_size_; @@ -176,7 +176,6 @@ private: float threshold_; unsigned int doppler_max_; unsigned int doppler_step_; - unsigned int sampled_ms_; unsigned int max_dwells_; int64_t fs_in_; bool dump_; @@ -185,6 +184,7 @@ private: std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; + unsigned int num_codes_; unsigned int in_streams_; unsigned int out_streams_; float calculate_threshold(float pfa); diff --git a/src/algorithms/libs/gnss_sdr_flags.cc b/src/algorithms/libs/gnss_sdr_flags.cc index 871776a52..6e021b73c 100644 --- a/src/algorithms/libs/gnss_sdr_flags.cc +++ b/src/algorithms/libs/gnss_sdr_flags.cc @@ -67,7 +67,7 @@ DEFINE_int32(cn0_samples, 20, "Number of correlator outputs used for CN0 estimat DEFINE_int32(cn0_min, 25, "Minimum valid CN0 (in dB-Hz)."); -DEFINE_int32(max_carrier_lock_fail, 10000, "Maximum number of carrier lock failures before dropping a satellite."); +DEFINE_int32(max_carrier_lock_fail, 5000, "Maximum number of carrier lock failures before dropping a satellite."); DEFINE_int32(max_lock_fail, 50, "Maximum number of code lock failures before dropping a satellite."); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 0ca5f5e74..0f56d1614 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -29,10 +29,10 @@ * * ------------------------------------------------------------------------- */ - - #include "beidou_b1i_telemetry_decoder_gs.h" #include "Beidou_B1I.h" +#include "Beidou_DNAV.h" +#include "beidou_dnav_almanac.h" #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" @@ -56,7 +56,6 @@ beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) return beidou_b1i_telemetry_decoder_gs_sptr(new beidou_b1i_telemetry_decoder_gs(satellite, dump)); } - beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( const Gnss_Satellite &satellite, bool dump) : gr::block("beidou_b1i_telemetry_decoder_gs", @@ -72,6 +71,7 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite; + d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); @@ -94,12 +94,15 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; d_symbol_history.set_capacity(d_required_symbols); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; d_preamble_index = 0; d_flag_frame_sync = false; - d_TOW_at_current_symbol_ms = 0; + d_TOW_at_current_symbol_ms = 0U; d_TOW_at_Preamble_ms = 0U; Flag_valid_word = false; d_CRC_error_counter = 0; @@ -150,7 +153,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in err = errind[reg[0] + reg[1] * 2 + reg[2] * 4 + reg[3] * 8]; - if (err > 0) + if (err > 0 and err < 16) { decbits[err - 1] *= -1; } @@ -230,11 +233,13 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // 3. Check operation executed correctly if (d_nav.flag_crc_test == true) { - LOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel + << " from satellite " << d_satellite; } else { - LOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel + << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) @@ -296,6 +301,36 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + // Setting samples of preamble code + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + d_preamble_samples[i] = 1; + } + else + { + d_preamble_samples[i] = -1; + } + } + + d_symbol_duration_ms = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); + } + else + { + // Clear values from previous declaration + volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_subframe_symbols); + //back to normal satellites + d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + // Setting samples of preamble code for (int32_t i = 0; i < d_symbols_per_preamble; i++) { @@ -342,6 +377,15 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel) } } +void beidou_b1i_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_TOW_at_current_symbol_ms = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; + DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite; + return; +} int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -358,10 +402,8 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); - d_flag_preamble = false; - //std::cout << "size: " << d_symbol_history.size() << " in " << current_symbol.Prompt_I << std::endl; if (d_symbol_history.size() >= d_required_symbols) { //******* preamble correlation ******** @@ -375,14 +417,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { corr_value += d_preamble_samples[i]; } - //std::cout << "corr: " << corr_value << ","; } - //std::cout << " final corr: " << corr_value << std::endl; - } - - if (abs(corr_value) >= d_samples_per_preamble) - { - std::cout << " preamble corr: " << corr_value << std::endl; } //******* frame sync ****************** if (d_stat == 0) // no preamble information @@ -391,7 +426,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - LOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; + DLOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -405,9 +440,54 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; + DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; d_preamble_index = d_sample_counter; //record the preamble sample stamp + + d_stat = 2; + + // ******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) //normal PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = d_symbol_history.at(i); + } + } + else // 180 deg. inverted carrier phase PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = -d_symbol_history.at(i); + } + } + + // call the decoder + decode_subframe(d_subframe_symbols); + + if (d_nav.flag_crc_test == true) + { + d_CRC_error_counter = 0; + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + if (!d_flag_frame_sync) + { + d_flag_frame_sync = true; + DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " << this->d_satellite; + } + } + else + { + d_CRC_error_counter++; + d_preamble_index = d_sample_counter; // record the preamble sample stamp + if (d_CRC_error_counter > CRC_ERROR_LIMIT) + { + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; + d_flag_frame_sync = false; + d_stat = 0; + flag_SOW_set = false; + } + } } else { @@ -428,36 +508,17 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = d_symbol_history.at(i); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = d_symbol_history.at(i); - } + d_subframe_symbols[i] = d_symbol_history.at(i); } } else // 180 deg. inverted carrier phase PLL lock { for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = -d_symbol_history.at(i); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = -d_symbol_history.at(i); - } + d_subframe_symbols[i] = -d_symbol_history.at(i); } } - // call the decoder decode_subframe(d_subframe_symbols); @@ -478,7 +539,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -493,52 +554,67 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * BEIDOU_B1I_CODE_PERIOD_MS); + //check TOW update consistency + uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + //compute new TOW + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; flag_SOW_set = true; d_nav.flag_new_SOW_available = false; - } - else // if there is not a new preamble, we define the TOW of the current symbol - { - d_TOW_at_current_symbol_ms += static_cast(BEIDOU_B1I_CODE_PERIOD_MS); - } + if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) + { + LOG(INFO) << "Warning: BEIDOU B1I TOW update in ch " << d_channel + << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; - if (d_flag_frame_sync == true and flag_SOW_set == true) - { - current_symbol.Flag_valid_word = true; + d_TOW_at_current_symbol_ms = 0; + d_flag_valid_word = false; + } + else + { + d_last_valid_preamble = d_sample_counter; + d_flag_valid_word = true; + } } else { - current_symbol.Flag_valid_word = false; + if (d_flag_valid_word) + { + d_TOW_at_current_symbol_ms += d_symbol_duration_ms; + if (current_symbol.Flag_valid_symbol_output == false) + { + d_flag_valid_word = false; + } + } } - current_symbol.PRN = this->d_satellite.get_PRN(); - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - - if (d_dump == true) + if (d_flag_valid_word == true) { - // MULTIPLEXED FILE RECORDING - Record results to file - try + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + current_symbol.Flag_valid_word = d_flag_valid_word; + + if (d_dump == true) { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = d_nav.d_SOW; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = static_cast(d_required_symbols); - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); + } } + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + return 1; } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - - return 1; + return 0; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 9180a40b8..9485008fd 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -62,10 +62,8 @@ public: ~beidou_b1i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - inline void reset() - { - return; - } + void reset(); + /*! * \brief This is where all signal processing takes place */ @@ -105,12 +103,16 @@ private: //!< Navigation Message variable Beidou_Dnav_Navigation_Message d_nav; - //!< Values to populate gnss synchronization structure + // Values to populate gnss synchronization structure + uint32_t d_symbol_duration_ms; uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; + uint64_t d_last_valid_preamble; + bool d_flag_valid_word; + bool d_sent_tlm_failed_msg; bool Flag_valid_word; - //!< Satellite Information and logging capacity + // Satellite Information and logging capacity Gnss_Satellite d_satellite; int32_t d_channel; bool d_dump; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 9f1396b8b..2e6d6b6af 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -52,8 +52,7 @@ beidou_b3i_telemetry_decoder_gs_sptr beidou_b3i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) { - return beidou_b3i_telemetry_decoder_gs_sptr( - new beidou_b3i_telemetry_decoder_gs(satellite, dump)); + return beidou_b3i_telemetry_decoder_gs_sptr(new beidou_b3i_telemetry_decoder_gs(satellite, dump)); } @@ -67,74 +66,37 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); - // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " - << this->d_satellite; + LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " << this->d_satellite; - d_samples_per_symbol = - (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / - BEIDOU_D1NAV_SYMBOL_RATE_SPS; + d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = - BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = static_cast( - volk_gnsssdr_malloc(BEIDOU_B3I_SECONDARY_CODE_LENGTH * sizeof(int32_t), - volk_gnsssdr_get_alignment())); - d_preamble_samples = static_cast(volk_gnsssdr_malloc( - d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - - // Setting samples of secondary code - for (int32_t i = 0; i < BEIDOU_B3I_SECONDARY_CODE_LENGTH; i++) - { - if (BEIDOU_B3I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_symbols[i] = 1; - } - else - { - d_secondary_code_symbols[i] = -1; - } - } + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - int32_t m = 0; if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast( - volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), - volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + - d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; @@ -153,7 +115,6 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -164,8 +125,7 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() } catch (const std::exception &ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " - << ex.what(); + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } } @@ -194,7 +154,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, err = errind[reg[0] + reg[1] * 2 + reg[2] * 4 + reg[3] * 8]; - if (err > 0) + if (err > 0 and err < 16) { decbits[err - 1] *= -1; } @@ -202,7 +162,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, void beidou_b3i_telemetry_decoder_gs::decode_word( - int32_t word_counter, const double *enc_word_symbols, + int32_t word_counter, + const float *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -211,8 +172,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t j = 0; j < 30; j++) { - dec_word_symbols[j] = - static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); + dec_word_symbols[j] = static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); } } else @@ -221,8 +181,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t c = 0; c < 15; c++) { - bitsbch[r * 15 + c] = - static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); + bitsbch[r * 15 + c] = static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); } } @@ -244,7 +203,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( } -void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) +void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -275,13 +234,13 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) // 3. Check operation executed correctly if (d_nav.flag_crc_test == true) { - LOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel + << " from satellite " << d_satellite; } else { - LOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel + << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) @@ -353,50 +312,62 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite( { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); - d_samples_per_symbol = - (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / - BEIDOU_D2NAV_SYMBOL_RATE_SPS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = - BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = nullptr; - d_preamble_samples = static_cast( - volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), - volk_gnsssdr_get_alignment())); - d_preamble_period_samples = - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), + volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } + d_preamble_samples[i] = -1; + } + } + d_symbol_duration_ms = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; + d_subframe_symbols = static_cast(volk_gnsssdr_malloc( + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), + volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); + } + else + { + // Clear values from previous declaration + volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_subframe_symbols); + //back to normal satellites + d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + + // Setting samples of preamble code + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + d_preamble_samples[i] = 1; + } + else + { + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc( - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), - volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + - d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); } } @@ -431,6 +402,15 @@ void beidou_b3i_telemetry_decoder_gs::set_channel(int32_t channel) } } +void beidou_b3i_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_TOW_at_current_symbol_ms = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; + DLOG(INFO) << "Beidou B3I Telemetry decoder reset for satellite " << d_satellite; + return; +} int beidou_b3i_telemetry_decoder_gs::general_work( int noutput_items __attribute__((unused)), @@ -451,10 +431,9 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); - d_flag_preamble = false; - if (d_symbol_history.size() > d_required_symbols) + if (d_symbol_history.size() >= d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -469,7 +448,6 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } - //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -477,8 +455,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - LOG(INFO) << "Preamble detection for BEIDOU B3I SAT " - << this->d_satellite; + DLOG(INFO) << "Preamble detection for BEIDOU B3I SAT " << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -492,10 +469,55 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " - << this->d_satellite; + DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " + << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp d_stat = 2; + + // ******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) //normal PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = d_symbol_history.at(i); + } + } + else // 180 deg. inverted carrier phase PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = -d_symbol_history.at(i); + } + } + + // call the decoder + decode_subframe(d_subframe_symbols); + + if (d_nav.flag_crc_test == true) + { + d_CRC_error_counter = 0; + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + if (!d_flag_frame_sync) + { + d_flag_frame_sync = true; + DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " + << this->d_satellite; + } + } + else + { + d_CRC_error_counter++; + d_preamble_index = d_sample_counter; // record the preamble sample stamp + if (d_CRC_error_counter > CRC_ERROR_LIMIT) + { + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " + << this->d_satellite; + d_flag_frame_sync = false; + d_stat = 0; + flag_SOW_set = false; + } + } } else { @@ -510,62 +532,21 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } else if (d_stat == 2) // preamble acquired { - if (d_sample_counter == - d_preamble_index + static_cast(d_preamble_period_samples)) + if (d_sample_counter == d_preamble_index + static_cast(d_preamble_period_samples)) { - //******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) // normal PLL lock + // ******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) //normal PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) - { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += - d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += - static_cast(d_secondary_code_symbols[k]) * - d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } - } + d_subframe_symbols[i] = d_symbol_history.at(i); } } else // 180 deg. inverted carrier phase PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) - { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= - d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= - static_cast(d_secondary_code_symbols[k]) * - d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } - } + d_subframe_symbols[i] = -d_symbol_history.at(i); } } @@ -575,10 +556,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (d_nav.flag_crc_test == true) { d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to - // false every work()) - d_preamble_index = - d_sample_counter; // record the preamble sample stamp (t_P) + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) if (!d_flag_frame_sync) { d_flag_frame_sync = true; @@ -592,8 +571,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " - << this->d_satellite; + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " + << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -601,7 +580,6 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } - // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) @@ -609,55 +587,67 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - d_TOW_at_current_symbol_ms = - d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * - BEIDOU_B3I_CODE_PERIOD_MS); + //check TOW update consistency + uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + //compute new TOW + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; flag_SOW_set = true; d_nav.flag_new_SOW_available = false; - } - else // if there is not a new preamble, we define the TOW of the current - // symbol - { - d_TOW_at_current_symbol_ms += - static_cast(BEIDOU_B3I_CODE_PERIOD_MS); - } - if (d_flag_frame_sync == true and flag_SOW_set == true) - { - current_symbol.Flag_valid_word = true; + if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) + { + LOG(INFO) << "Warning: BEIDOU B3I TOW update in ch " << d_channel + << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; + + d_TOW_at_current_symbol_ms = 0; + d_flag_valid_word = false; + } + else + { + d_last_valid_preamble = d_sample_counter; + d_flag_valid_word = true; + } } else { - current_symbol.Flag_valid_word = false; + if (d_flag_valid_word) + { + d_TOW_at_current_symbol_ms += d_symbol_duration_ms; + if (current_symbol.Flag_valid_symbol_output == false) + { + d_flag_valid_word = false; + } + } } - current_symbol.PRN = this->d_satellite.get_PRN(); - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - - if (d_dump == true) + if (d_flag_valid_word == true) { - // MULTIPLEXED FILE RECORDING - Record results to file - try + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + current_symbol.Flag_valid_word = d_flag_valid_word; + + if (d_dump == true) { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = d_nav.d_SOW; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = static_cast(d_required_symbols); - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); + } } + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + return 1; } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - - return 1; + return 0; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h index 54bac3adb..d08f8388e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h @@ -60,10 +60,8 @@ public: ~beidou_b3i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - inline void reset() - { - return; - } + void reset(); + /*! * \brief This is where all signal processing takes place */ @@ -77,27 +75,24 @@ private: bool dump); beidou_b3i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(double *symbols); - void decode_word(int32_t word_counter, const double *enc_word_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_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; - int32_t *d_secondary_code_symbols; - uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - double *d_subframe_symbols; + float *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data boost::circular_buffer d_symbol_history; // Variables for internal functionality - uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) - // indicating number of samples processed + uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed uint64_t d_preamble_index; // Index of sample number where preamble was found uint32_t d_stat; // Status of decoder bool d_flag_frame_sync; // Indicate when a frame sync is achieved @@ -109,8 +104,12 @@ private: Beidou_Dnav_Navigation_Message d_nav; // Values to populate gnss synchronization structure + uint32_t d_symbol_duration_ms; uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; + uint64_t d_last_valid_preamble; + bool d_flag_valid_word; + bool d_sent_tlm_failed_msg; bool Flag_valid_word; // Satellite Information and logging capacity diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 4bfcc06d3..254993c49 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -112,7 +112,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; - d_max_symbols_without_valid_frame = GALILEO_FNAV_SYMBOLS_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_FNAV_SYMBOLS_PER_PAGE * 5; //rise alarm 100 seconds without valid tlm break; } default: @@ -623,12 +623,11 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( return -1; break; } - + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) if (d_inav_nav.flag_CRC_test == true or d_fnav_nav.flag_CRC_test == true) { d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) d_last_valid_preamble = d_sample_counter; if (!d_flag_frame_sync) { @@ -639,7 +638,6 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( else { d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 612045e31..3e406a544 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -76,33 +76,26 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( DLOG(INFO) << "Initializing GPS L1 TELEMETRY DECODER"; d_bits_per_preamble = GPS_CA_PREAMBLE_LENGTH_BITS; - //d_samples_per_preamble = d_bits_per_preamble * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_samples_per_preamble = d_bits_per_preamble; - d_preamble_period_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_preamble_period_symbols = GPS_SUBFRAME_BITS; // set the preamble - d_required_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_required_symbols = GPS_SUBFRAME_BITS; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; - d_max_symbols_without_valid_frame = d_required_symbols * 10; // rise alarm 1 minute without valid tlm + d_max_symbols_without_valid_frame = d_required_symbols * 20; // rise alarm 120 segs without valid tlm int32_t n = 0; for (int32_t i = 0; i < d_bits_per_preamble; i++) { if (GPS_CA_PREAMBLE.at(i) == '1') { - // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - // { d_preamble_samples[n] = 1; n++; - // } } else { - // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - // { d_preamble_samples[n] = -1; n++; - // } } } d_sample_counter = 0ULL; @@ -462,7 +455,6 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ else { d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > 2) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; diff --git a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc index fb998ab81..c311d1ef2 100644 --- a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc @@ -58,22 +58,39 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( trk_param.fs_in = fs_in; bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; + std::string default_dump_filename = "./track_ch"; + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + trk_param.dump_filename = dump_filename; + bool dump_mat = configuration->property(role + ".dump_mat", true); + trk_param.dump_mat = dump_mat; + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: BEIDOU B3I. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); + } float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) { pll_bw_hz = static_cast(FLAGS_pll_bw_hz); } trk_param.pll_bw_hz = pll_bw_hz; - float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); - trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; - float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); - trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) { dll_bw_hz = static_cast(FLAGS_dll_bw_hz); } trk_param.dll_bw_hz = dll_bw_hz; + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + trk_param.early_late_space_chips = early_late_space_chips; int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); if (dll_filter_order < 1) @@ -116,16 +133,12 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( trk_param.fll_bw_hz = fll_bw_hz; trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); - float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); - trk_param.early_late_space_chips = early_late_space_chips; - float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); - trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; - std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); - trk_param.dump_filename = dump_filename; - int vector_length = std::round(fs_in / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS)); + int vector_length = std::round(static_cast(fs_in) / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS)); trk_param.vector_length = vector_length; int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); + trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; + bool track_pilot = configuration->property(role + ".track_pilot", false); if (symbols_extended_correlator < 1) { symbols_extended_correlator = 1; @@ -137,18 +150,9 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( std::cout << TEXT_RED << "WARNING: BEIDOU B3I. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } trk_param.extend_correlation_symbols = symbols_extended_correlator; - bool track_pilot = configuration->property(role + ".track_pilot", false); - if (track_pilot) - { - std::cout << TEXT_RED << "WARNING: BEIDOU B3I does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; - } - if ((symbols_extended_correlator > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) - { - std::cout << TEXT_RED << "WARNING: BEIDOU B3I. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; - } + trk_param.track_pilot = track_pilot; trk_param.very_early_late_space_chips = 0.0; trk_param.very_early_late_space_narrow_chips = 0.0; - trk_param.track_pilot = false; trk_param.system = 'C'; char sig_[3] = "B3"; std::memcpy(trk_param.signal, sig_, 3); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 1a03f987d..a50b02310 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -102,7 +102,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking::msg_handler_telemetry_to_trk, this, _1)); // initialize internal vars - d_dll_filt_history.set_capacity(2000); + d_dll_filt_history.set_capacity(1000); d_veml = false; d_cloop = true; d_pull_in_transitory = true; @@ -546,7 +546,7 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); - d_carrier_lock_fail_counter = 100000; //force loss-of-lock condition + d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition break; } default: @@ -645,18 +645,32 @@ void dll_pll_veml_tracking::start_tracking() else if (systemName == "Beidou" and signal_type == "B1") { beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); - // Update secondary code settings for geo satellites + // GEO Satellites use different secondary code if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { - d_symbols_per_bit = 2; + d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - // preamble bits to sampled symbols // set the preamble in the secondary code acquisition d_secondary_code_length = static_cast(BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS); d_secondary_code_string = const_cast(&BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR); + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -667,15 +681,29 @@ void dll_pll_veml_tracking::start_tracking() // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { - d_symbols_per_bit = 2; + d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - // preamble bits to sampled symbols // set the preamble in the secondary code acquisition d_secondary_code_length = static_cast(BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS); d_secondary_code_string = const_cast(&BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR); + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -976,16 +1004,18 @@ void dll_pll_veml_tracking::run_dll_pll() if (d_pull_in_transitory == false and d_corrected_doppler == false) { d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + if (d_dll_filt_history.full()) { - float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0) / static_cast(d_dll_filt_history.capacity()); - if (fabs(avg_code_error_chips_s) > 1.0) + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 0.025) { float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); d_corrected_doppler = true; } + d_dll_filt_history.clear(); } } } @@ -1158,7 +1188,7 @@ void dll_pll_veml_tracking::save_correlation_results() d_P_data_accu -= *d_Prompt; } } - // std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + d_current_data_symbol++; // data secondary code roll-up d_current_data_symbol %= d_data_secondary_code_length; @@ -1172,6 +1202,7 @@ void dll_pll_veml_tracking::save_correlation_results() else { d_P_data_accu += *d_Prompt; + //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; } d_current_data_symbol++; d_current_data_symbol %= d_symbols_per_bit; @@ -1614,6 +1645,13 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_P_accu = *d_Prompt; d_L_accu = *d_Late; + //fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period + if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } // Check lock status if (!cn0_and_tracking_lock_status(d_code_period)) { @@ -1730,6 +1768,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) update_tracking_vars(); if (d_current_data_symbol == 0) { + log_data(); // ########### Output the tracking results to Telemetry block ########## // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -1769,6 +1808,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) update_tracking_vars(); if (d_current_data_symbol == 0) { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); // ########### Output the tracking results to Telemetry block ########## // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -1782,8 +1823,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) current_synchro_data.Flag_valid_symbol_output = true; d_P_data_accu = gr_complex(0.0, 0.0); } - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 8bb114e96..f31bccc4e 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -47,6 +47,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() enable_fll_pull_in = false; enable_fll_steady_state = false; pull_in_time_s = 10; + bit_synchronization_time_limit_s = pull_in_time_s + 60; fll_filter_order = 1; pll_filter_order = 3; dll_filter_order = 2; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index 94ac5e270..d84e8f204 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -45,6 +45,7 @@ public: bool enable_fll_pull_in; bool enable_fll_steady_state; unsigned int pull_in_time_s; + unsigned int bit_synchronization_time_limit_s; int pll_filter_order; int dll_filter_order; diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index 48dd4ea78..bdc5ff40f 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -67,6 +67,7 @@ const double BEIDOU_B1I_PREAMBLE_DURATION_S = 0.220; const int BEIDOU_B1I_PREAMBLE_DURATION_MS = 220; const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; +const int BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT = 2; const int BEIDOU_B1I_TELEMETRY_SYMBOL_PERIOD_MS = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; const int BEIDOU_B1I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND * BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] const int BEIDOU_WORD_LENGTH = 4; //**************!< CRC + BEIDOU WORD (-2 -1 0 ... 29) Bits = 4 bytes diff --git a/src/core/system_parameters/Beidou_B3I.h b/src/core/system_parameters/Beidou_B3I.h index bd58aeae0..3eceab185 100644 --- a/src/core/system_parameters/Beidou_B3I.h +++ b/src/core/system_parameters/Beidou_B3I.h @@ -52,8 +52,9 @@ const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_BITS = 11; const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_SYMBOLS = 220; // ************** const double BEIDOU_B3I_PREAMBLE_DURATION_S = 0.220; const int32_t BEIDOU_B3I_PREAMBLE_DURATION_MS = 220; -const int32_t BEIDOU_B3I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] -const int32_t BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT = 20; // ************* +const int32_t BEIDOU_B3I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] +const int32_t BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT = 20; +const int32_t BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT = 2; // ************* const int32_t BEIDOU_B3I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B3I_TELEMETRY_RATE_BITS_SECOND * BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] #endif /* GNSS_SDR_BEIDOU_B3I_H_ */ From 4de86b1f8aa30478a1f02992e8945b8be87fab33 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 3 Jul 2019 23:47:42 +0200 Subject: [PATCH 38/64] Use std::array instead of pointers and memcpy --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 2 +- src/core/monitor/serdes_gnss_synchro.h | 27 +++++++++---------- .../unit-tests/control-plane/protobuf_test.cc | 5 +++- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 7b3fdafc5..490580818 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -50,6 +50,7 @@ public: GOOGLE_PROTOBUF_VERIFY_VERSION; monitor_.New(); } + ~Serdes_Monitor_Pvt() { // google::protobuf::ShutdownProtobufLibrary(); @@ -94,7 +95,6 @@ public: return data; } - inline Monitor_Pvt readProtobuffer(const gnss_sdr::MonitorPvt& mon) //!< Deserialization { Monitor_Pvt monitor; diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index ee79ce993..3f18a3ac2 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -34,7 +34,7 @@ #include "gnss_synchro.h" #include "gnss_synchro.pb.h" // file created by Protocol Buffers at compile time -#include // for memcpy +#include #include @@ -52,6 +52,7 @@ public: GOOGLE_PROTOBUF_VERIFY_VERSION; observables.New(); } + ~Serdes_Gnss_Synchro() { google::protobuf::ShutdownProtobufLibrary(); @@ -60,21 +61,17 @@ public: inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { observables.Clear(); - std::string data; for (auto gs : vgs) { gnss_sdr::GnssSynchro* obs = observables.add_observable(); - char c[2]; - c[0] = gs.System; - c[1] = '\0'; - const std::string sys(c); + char c = gs.System; + const std::string sys(1, c); - char cc[3]; + std::array cc; cc[0] = gs.Signal[0]; cc[1] = gs.Signal[1]; - cc[2] = '\0'; - const std::string sig(cc); + const std::string sig(cc.cbegin(), cc.cend()); obs->set_system(sys); obs->set_signal(sig); @@ -109,20 +106,19 @@ public: return data; } - - inline std::vector readProtobuffer(const gnss_sdr::Observables& obs) //!< Deserialization + inline std::vector readProtobuffer(const gnss_sdr::Observables& obs) const //!< Deserialization { std::vector vgs; vgs.reserve(obs.observable_size()); - for (int i = 0; i < obs.observable_size(); ++i) { const gnss_sdr::GnssSynchro& gs_read = obs.observable(i); Gnss_Synchro gs = Gnss_Synchro(); const std::string& sys = gs_read.system(); - gs.System = *sys.c_str(); - const std::string& sig = gs_read.signal(); - std::memcpy(static_cast(gs.Signal), sig.c_str(), 3); + gs.System = gs_read.system()[0]; + gs.Signal[0] = gs_read.signal()[0]; + gs.Signal[1] = gs_read.signal()[1]; + gs.Signal[2] = '\0'; gs.PRN = gs_read.prn(); gs.Channel_ID = gs_read.channel_id(); @@ -153,6 +149,7 @@ public: } return vgs; } + private: gnss_sdr::Observables observables; }; diff --git a/src/tests/unit-tests/control-plane/protobuf_test.cc b/src/tests/unit-tests/control-plane/protobuf_test.cc index 95d83a519..9bece14eb 100644 --- a/src/tests/unit-tests/control-plane/protobuf_test.cc +++ b/src/tests/unit-tests/control-plane/protobuf_test.cc @@ -95,12 +95,15 @@ TEST(Protobuf, Works) Gnss_Synchro gs_read = vgs_read[0]; uint32_t prn_read = gs_read.PRN; uint32_t prn_read2 = vgs_read[1].PRN; + std::string system_read(1, gs_read.System); + std::string signal_read(gs_read.Signal); // or without the need of gnss_synchro: int obs_size = obs.observable_size(); uint32_t prn_read3 = obs.observable(0).prn(); - EXPECT_EQ(prn_true, prn_read); + EXPECT_EQ(sig, signal_read); + EXPECT_EQ(sys, system_read); EXPECT_EQ(prn_true2, prn_read2); EXPECT_EQ(prn_true, prn_read3); EXPECT_EQ(2, obs_size); From 9ba6f4d6709b025e63178c6a7b76654131a5d03a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 4 Jul 2019 00:00:29 +0200 Subject: [PATCH 39/64] Make header file more readable. Remove unused private method interpolate_data() --- .../gnuradio_blocks/hybrid_observables_gs.h | 56 +++++++++++-------- 1 file changed, 33 insertions(+), 23 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index b40beada9..f21510254 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -34,7 +34,7 @@ #ifndef GNSS_SDR_HYBRID_OBSERVABLES_GS_H #define GNSS_SDR_HYBRID_OBSERVABLES_GS_H -#include // for boost::curcular_buffer +#include // for boost::circular_buffer #include // for boost::shared_ptr #include // for block #include // for gr_vector_int @@ -50,8 +50,12 @@ class Gnss_circular_deque; using hybrid_observables_gs_sptr = boost::shared_ptr; -hybrid_observables_gs_sptr -hybrid_observables_gs_make(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, bool dump_mat, std::string dump_filename); +hybrid_observables_gs_sptr hybrid_observables_gs_make( + unsigned int nchannels_in, + unsigned int nchannels_out, + bool dump, + bool dump_mat, + std::string dump_filename); /*! * \brief This class implements a block that computes observables @@ -60,38 +64,44 @@ class hybrid_observables_gs : public gr::block { public: ~hybrid_observables_gs(); + void forecast(int noutput_items, gr_vector_int& ninput_items_required); int general_work(int noutput_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); - void forecast(int noutput_items, gr_vector_int& ninput_items_required); private: - friend hybrid_observables_gs_sptr - hybrid_observables_gs_make(uint32_t nchannels_in, uint32_t nchannels_out, bool dump, bool dump_mat, std::string dump_filename); - hybrid_observables_gs(uint32_t nchannels_in, uint32_t nchannels_out, bool dump, bool dump_mat, std::string dump_filename); - void msg_handler_pvt_to_observables(const pmt::pmt_t& msg); - bool interpolate_data(Gnss_Synchro& out, const uint32_t& ch, const double& ti); - bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const uint32_t& ch, const uint64_t& rx_clock); - double compute_T_rx_s(const Gnss_Synchro& a); - void compute_pranges(std::vector& data); - void update_TOW(const std::vector& data); - int32_t save_matfile(); + friend hybrid_observables_gs_sptr hybrid_observables_gs_make( + uint32_t nchannels_in, + uint32_t nchannels_out, + bool dump, + bool dump_mat, + std::string dump_filename); - //time history - boost::circular_buffer d_Rx_clock_buffer; - //Tracking observable history - Gnss_circular_deque* d_gnss_synchro_history; - //rx time follow GPST - bool T_rx_TOW_set; + hybrid_observables_gs( + uint32_t nchannels_in, + uint32_t nchannels_out, + bool dump, + bool dump_mat, + std::string dump_filename); + + bool T_rx_TOW_set; // rx time follow GPST + bool d_dump; + bool d_dump_mat; uint32_t T_rx_TOW_ms; uint32_t T_rx_remnant_to_20ms; uint32_t T_rx_step_ms; - double T_rx_offset_ms; - bool d_dump; - bool d_dump_mat; uint32_t d_nchannels_in; uint32_t d_nchannels_out; + double T_rx_offset_ms; std::string d_dump_filename; std::ofstream d_dump_file; + boost::circular_buffer d_Rx_clock_buffer; // time history + Gnss_circular_deque* d_gnss_synchro_history; // Tracking observable history + void msg_handler_pvt_to_observables(const pmt::pmt_t& msg); + double compute_T_rx_s(const Gnss_Synchro& a); + bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const uint32_t& ch, const uint64_t& rx_clock); + void update_TOW(const std::vector& data); + void compute_pranges(std::vector& data); + int32_t save_matfile(); }; #endif From fd9edf248639ec468a46e33998f3b3d4a716b1b2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 4 Jul 2019 08:06:02 +0200 Subject: [PATCH 40/64] Remove duplicate character if GROSMOSDR_PKG_VERSION contains a v --- cmake/Modules/FindGROSMOSDR.cmake | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cmake/Modules/FindGROSMOSDR.cmake b/cmake/Modules/FindGROSMOSDR.cmake index 78fb58a31..175ef6c58 100644 --- a/cmake/Modules/FindGROSMOSDR.cmake +++ b/cmake/Modules/FindGROSMOSDR.cmake @@ -101,6 +101,9 @@ find_package_handle_standard_args(GROSMOSDR DEFAULT_MSG GROSMOSDR_LIBRARIES GROS if(GROSMOSDR_PKG_VERSION) set(GROSMOSDR_VERSION ${GROSMOSDR_PKG_VERSION}) + set(KK "v1.2.3") + string(REGEX REPLACE "^v" "" KK_AUX KK) + message(STATUS "++++++++++++++++++++${KK_AUX}") endif() set_package_properties(GROSMOSDR PROPERTIES From 2d276526a81f5b25e93a19e512548243e0709860 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 4 Jul 2019 08:11:25 +0200 Subject: [PATCH 41/64] Remove duplicate character if GROSMOSDR_PKG_VERSION contains a v (Fix) --- cmake/Modules/FindGROSMOSDR.cmake | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/cmake/Modules/FindGROSMOSDR.cmake b/cmake/Modules/FindGROSMOSDR.cmake index 175ef6c58..ce2e655b5 100644 --- a/cmake/Modules/FindGROSMOSDR.cmake +++ b/cmake/Modules/FindGROSMOSDR.cmake @@ -100,10 +100,8 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(GROSMOSDR DEFAULT_MSG GROSMOSDR_LIBRARIES GROSMOSDR_INCLUDE_DIR) if(GROSMOSDR_PKG_VERSION) - set(GROSMOSDR_VERSION ${GROSMOSDR_PKG_VERSION}) - set(KK "v1.2.3") - string(REGEX REPLACE "^v" "" KK_AUX KK) - message(STATUS "++++++++++++++++++++${KK_AUX}") + set(GROSMOSDR_VERSION_AUX ${GROSMOSDR_PKG_VERSION}) + string(REGEX REPLACE "^v" "" GROSMOSDR_VERSION ${GROSMOSDR_VERSION_AUX}) endif() set_package_properties(GROSMOSDR PROPERTIES From ad389cf2677c3174a2efae966a11b97dc58a08b5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 4 Jul 2019 08:24:48 +0200 Subject: [PATCH 42/64] Remove unused variable --- src/core/monitor/serdes_gnss_synchro.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 3f18a3ac2..0e63aed6c 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -114,7 +114,6 @@ public: { const gnss_sdr::GnssSynchro& gs_read = obs.observable(i); Gnss_Synchro gs = Gnss_Synchro(); - const std::string& sys = gs_read.system(); gs.System = gs_read.system()[0]; gs.Signal[0] = gs_read.signal()[0]; gs.Signal[1] = gs_read.signal()[1]; From 620191f818f5500f87c384ec55613d7593b6a0a2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 4 Jul 2019 08:52:06 +0200 Subject: [PATCH 43/64] Fix warning (unused variable) --- src/tests/unit-tests/control-plane/protobuf_test.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/tests/unit-tests/control-plane/protobuf_test.cc b/src/tests/unit-tests/control-plane/protobuf_test.cc index 9bece14eb..671150717 100644 --- a/src/tests/unit-tests/control-plane/protobuf_test.cc +++ b/src/tests/unit-tests/control-plane/protobuf_test.cc @@ -106,5 +106,6 @@ TEST(Protobuf, Works) EXPECT_EQ(sys, system_read); EXPECT_EQ(prn_true2, prn_read2); EXPECT_EQ(prn_true, prn_read3); + EXPECT_EQ(prn_read, prn_read3); EXPECT_EQ(2, obs_size); } From 7877754cb28be88e2534634e11a229ee0d2c6b8d Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 4 Jul 2019 12:16:37 +0200 Subject: [PATCH 44/64] Debug gnuradio buffer latency --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc | 4 ++++ .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 +++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 3e406a544..90a209fa5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -62,6 +62,10 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("gps_navigation_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + this->set_max_output_buffer(128); + LOG(INFO) << "current tlm output buffer set to " << this->max_output_buffer(0); + std::cout << "current tlm output buffer set to " << this->max_output_buffer(0) << "\n"; + // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index a50b02310..6ec7ccf02 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -92,6 +92,9 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + this->set_max_output_buffer(128); + LOG(INFO) << "current tracking output buffer set to " << this->max_output_buffer(0); + std::cout << "current tracking output buffer set to " << this->max_output_buffer(0) << "\n"; trk_parameters = conf_; // Telemetry bit synchronization message port input this->message_port_register_out(pmt::mp("events")); @@ -566,7 +569,6 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) void dll_pll_veml_tracking::start_tracking() { gr::thread::scoped_lock l(d_setlock); - // correct the code phase according to the delay between acq and trk d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; From 951e179bb85f501174c5e930a9895191be032d04 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 4 Jul 2019 12:54:45 +0200 Subject: [PATCH 45/64] Debug gnuradio buffer latency test 2 --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc | 5 ++--- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 +--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 90a209fa5..2fd99ae09 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -62,9 +62,8 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("gps_navigation_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - this->set_max_output_buffer(128); - LOG(INFO) << "current tlm output buffer set to " << this->max_output_buffer(0); - std::cout << "current tlm output buffer set to " << this->max_output_buffer(0) << "\n"; + //this->set_max_output_buffer(128); + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 6ec7ccf02..7450782b4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -92,9 +92,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - this->set_max_output_buffer(128); - LOG(INFO) << "current tracking output buffer set to " << this->max_output_buffer(0); - std::cout << "current tracking output buffer set to " << this->max_output_buffer(0) << "\n"; + this->set_max_noutput_items(1); trk_parameters = conf_; // Telemetry bit synchronization message port input this->message_port_register_out(pmt::mp("events")); From 29f13e5e3128dcc67112894d76ddbc807ac791ca Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 4 Jul 2019 17:19:33 +0200 Subject: [PATCH 46/64] Set noutput items limit in telemetry decoders and some minor changes --- .../beidou_b1i_telemetry_decoder_gs.cc | 11 +++++++---- .../beidou_b3i_telemetry_decoder_gs.cc | 19 +++++++++++-------- .../galileo_telemetry_decoder_gs.cc | 2 ++ .../glonass_l1_ca_telemetry_decoder_gs.cc | 2 ++ .../glonass_l2_ca_telemetry_decoder_gs.cc | 2 ++ .../gps_l1_ca_telemetry_decoder_gs.cc | 2 +- .../gps_l2c_telemetry_decoder_gs.cc | 2 ++ .../gps_l5_telemetry_decoder_gs.cc | 2 ++ .../sbas_l1_telemetry_decoder_gs.cc | 2 ++ .../gnuradio_blocks/dll_pll_veml_tracking.cc | 1 + 10 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 0f56d1614..b597504b3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -36,6 +36,7 @@ #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" +#include "display.h" #include "gnss_synchro.h" #include #include @@ -62,6 +63,8 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block @@ -248,7 +251,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_utc_model() == true) { @@ -256,7 +259,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_utc_model()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_iono() == true) { @@ -264,7 +267,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_iono()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << std::endl; + std::cout << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_almanac() == true) { @@ -272,7 +275,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // std::shared_ptr tmp_obj = std::make_shared(d_nav.get_almanac(slot_nbr)); // this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Almanac have been received in channel" << d_channel << " from satellite " << d_satellite << std::endl; - std::cout << "New BEIDOU B1I DNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << std::endl; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 2e6d6b6af..419aff764 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -35,6 +35,7 @@ #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" +#include "display.h" #include "gnss_synchro.h" #include #include @@ -62,6 +63,8 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block @@ -251,8 +254,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B3I DNAV message received in channel " << d_channel - << ": ephemeris from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV message received in channel " << d_channel + << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_utc_model() == true) { @@ -262,9 +265,9 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B3I DNAV utc model message received in channel " + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " - << d_satellite << std::endl; + << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_iono() == true) { @@ -274,9 +277,9 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B3I DNAV Iono message received in channel " + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " - << d_satellite << std::endl; + << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_almanac() == true) { @@ -287,8 +290,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Almanac have been received in channel" << d_channel << " from satellite " << d_satellite << std::endl; - std::cout << "New BEIDOU B3I DNAV almanac received in channel " << d_channel - << " from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV almanac received in channel " << d_channel + << " from satellite " << d_satellite << TEXT_RESET << std::endl; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 254993c49..965100242 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -66,6 +66,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( bool dump) : gr::block("galileo_telemetry_decoder_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 7629c2810..3f6fdabcb 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("glonass_l1_ca_telemetry_decoder_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index 144554491..e26592279 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( bool dump) : gr::block("glonass_l2_ca_telemetry_decoder_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 2fd99ae09..dc4eeed36 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -62,7 +62,7 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("gps_navigation_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - //this->set_max_output_buffer(128); + //prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); // Ephemeris data port out diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc index 44cd5cd40..e2702b374 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ gps_l2c_telemetry_decoder_gs::gps_l2c_telemetry_decoder_gs( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index 891f12c27..deccd3c54 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -58,6 +58,8 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc index cfa68d2be..daef06b6e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc @@ -58,6 +58,8 @@ sbas_l1_telemetry_decoder_gs::sbas_l1_telemetry_decoder_gs( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 7450782b4..4f324af8b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -92,6 +92,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); trk_parameters = conf_; // Telemetry bit synchronization message port input From 52c08e3ab474bfb6a0db321cad5334034a7df13b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 13:08:00 +0200 Subject: [PATCH 47/64] Add copy constructor, copy assignment operator, move constructor and move assignment operator --- src/core/system_parameters/gnss_satellite.cc | 255 +++++++++++-------- src/core/system_parameters/gnss_satellite.h | 8 +- 2 files changed, 154 insertions(+), 109 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index bfebef376..c928ce069 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -89,29 +89,60 @@ bool operator==(const Gnss_Satellite& sat1, const Gnss_Satellite& sat2) { if (sat1.get_PRN() == sat2.get_PRN()) { - equal = true; + if (sat1.get_rf_link() == sat2.get_rf_link()) + { + equal = true; + } } } return equal; } -/* -Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite &rhs) { +// Copy constructor +Gnss_Satellite::Gnss_Satellite(Gnss_Satellite&& other) +{ + *this = std::move(other); +} + + +// Copy assignment operator +Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) +{ // Only do assignment if RHS is a different object from this. - if (this != &rhs) { + if (this != &rhs) + { // Deallocate, allocate new space, copy values... - const std::string system_ = rhs.get_system(); - const uint32_t PRN_ = rhs.get_PRN(); - const std::string block_ = rhs.get_block(); - // const int32_t rf_link_ = 0; - this->set_system(system_); - this->set_PRN(PRN_); - this->set_block(system_, PRN_); - //this.rf_link = rf_link_; - } + this->reset(); + this->set_system(rhs.get_system()); + this->set_PRN(rhs.get_PRN()); + this->set_block(rhs.get_system(), rhs.get_PRN()); + this->set_rf_link(rhs.get_rf_link()); + } return *this; -}*/ +} + + +// Move constructor +Gnss_Satellite::Gnss_Satellite(const Gnss_Satellite& other) +{ + *this = std::move(other); +} + + +// Move assignment operator +Gnss_Satellite& Gnss_Satellite::operator=(Gnss_Satellite&& other) +{ + if (this != &other) + { + this->reset(); + this->system = std::move(other.get_system()); + this->PRN = std::move(other.get_PRN()); + this->block = std::move(other.get_block()); + this->rf_link = std::move(other.get_rf_link()); + } + return *this; +} void Gnss_Satellite::set_system(const std::string& system_) @@ -260,6 +291,14 @@ int32_t Gnss_Satellite::get_rf_link() const } +void Gnss_Satellite::set_rf_link(int32_t rf_link_) +{ + // Set satellite's rf link. Identifies the GLONASS Frequency Channel + rf_link = rf_link_; + return; +} + + uint32_t Gnss_Satellite::get_PRN() const { // Get satellite's PRN @@ -625,100 +664,100 @@ std::string Gnss_Satellite::what_block(const std::string& system_, uint32_t PRN_ if (system_ == "Beidou") { // Check https://en.wikipedia.org/wiki/List_of_BeiDou_satellites - switch ( PRN_ ) - { - case 1: - block_ = std::string("Compass-G1"); //! system_set; // = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}; void reset(); + void set_rf_link(int32_t rf_link_); }; #endif From 38b91bec13177515084eaf5590a58cee0aa76cfe Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 5 Jul 2019 13:29:23 +0200 Subject: [PATCH 48/64] Fix GPS L2CM tracking and tlm decoding --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 4f324af8b..2f46f0f95 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -1009,7 +1009,7 @@ void dll_pll_veml_tracking::run_dll_pll() if (d_dll_filt_history.full()) { float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); - if (fabs(avg_code_error_chips_s) > 0.025) + if (fabs(avg_code_error_chips_s) > 0.10) { float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); From 1237a29fc90f2e819ca1bddd4f663d83f4f8c25b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 14:42:48 +0200 Subject: [PATCH 49/64] Add copy constructor, copy assignment operator, move constructor and move assignment operator --- src/core/system_parameters/gnss_signal.cc | 40 +++++++++++++++++++++++ src/core/system_parameters/gnss_signal.h | 4 +++ 2 files changed, 44 insertions(+) diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index 0c25a7841..520d7242a 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -53,6 +53,46 @@ Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& si Gnss_Signal::~Gnss_Signal() = default; +// Copy constructor +Gnss_Signal::Gnss_Signal(Gnss_Signal&& other) +{ + *this = std::move(other); +} + + +// Copy assignment operator +Gnss_Signal& Gnss_Signal::operator=(const Gnss_Signal& rhs) +{ + // Only do assignment if RHS is a different object from this. + if (this != &rhs) + { + // Deallocate, allocate new space, copy values... + this->satellite = rhs.get_satellite(); + this->signal = rhs.get_signal_str(); + } + return *this; +} + + +// Move constructor +Gnss_Signal::Gnss_Signal(const Gnss_Signal& other) +{ + *this = std::move(other); +} + + +// Move assignment operator +Gnss_Signal& Gnss_Signal::operator=(Gnss_Signal&& other) +{ + if (this != &other) + { + this->satellite = std::move(other.get_satellite()); + this->signal = std::move(other.get_signal_str()); + } + return *this; +} + + std::string Gnss_Signal::get_signal_str() const { return this->signal; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index c2f6e37c4..92051444a 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -54,6 +54,10 @@ public: friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing + Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor + Gnss_Signal& operator=(const Gnss_Signal&); //!< Copy assignment operator + Gnss_Signal(const Gnss_Signal& other); //!< Move constructor + Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator private: Gnss_Satellite satellite; std::string signal; From 0e0991a1a55f843603adb26b1a2467e5fd3c542e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 14:44:56 +0200 Subject: [PATCH 50/64] Add move assignment operator --- .../tracking/libs/exponential_smoother.cc | 18 ++++++++++++++++++ .../tracking/libs/exponential_smoother.h | 1 + .../tracking/libs/tracking_loop_filter.cc | 19 +++++++++++++++++++ .../tracking/libs/tracking_loop_filter.h | 16 +++++++++------- 4 files changed, 47 insertions(+), 7 deletions(-) diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 204a1afb1..186882d06 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -51,6 +51,24 @@ Exponential_Smoother::Exponential_Smoother() Exponential_Smoother::~Exponential_Smoother() = default; +// Move assignment operator +Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) +{ + if (this != &other) + { + this->alpha_ = other.alpha_; + this->old_value_ = other.old_value_; + this->one_minus_alpha_ = 1.0 - other.alpha_; + this->samples_for_initialization_ = other.samples_for_initialization_; + this->initializing_ = other.initializing_; + this->init_counter_ = other.init_counter_; + this->min_value_ = other.min_value_; + this->offset_ = other.offset_; + } + return *this; +} + + void Exponential_Smoother::set_alpha(float alpha) { alpha_ = alpha; diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index f8ff21182..dc7f222d4 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -56,6 +56,7 @@ public: void set_offset(float offset); float smooth(float raw); double smooth(double raw); + Exponential_Smoother& operator=(Exponential_Smoother&& other); //!< Move assignment operator private: float alpha_; // takes value 0.0001 if not set int samples_for_initialization_; diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index 1d1b679c6..d2b581d2f 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -71,6 +71,25 @@ Tracking_loop_filter::Tracking_loop_filter() Tracking_loop_filter::~Tracking_loop_filter() = default; +// Move assignment operator +Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) +{ + if (this != &other) + { + this->d_inputs = other.d_inputs; + this->d_outputs = other.d_outputs; + this->d_input_coefficients = other.d_input_coefficients; + this->d_output_coefficients = other.d_output_coefficients; + this->d_loop_order = other.d_loop_order; + this->d_current_index = other.d_current_index; + this->d_include_last_integrator = other.d_include_last_integrator; + this->d_noise_bandwidth = other.d_noise_bandwidth; + this->d_update_interval = other.d_update_interval; + } + return *this; +} + + float Tracking_loop_filter::apply(float current_input) { // Now apply the filter coefficients: diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index 0151f3c5a..204b19825 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -44,6 +44,15 @@ class Tracking_loop_filter { public: + Tracking_loop_filter(); + ~Tracking_loop_filter(); + + Tracking_loop_filter(float update_interval, float noise_bandwidth, + int loop_order = 2, + bool include_last_integrator = false); + + Tracking_loop_filter& operator=(Tracking_loop_filter&& other); //!< Move assignment operator + float get_noise_bandwidth(void) const; float get_update_interval(void) const; bool get_include_last_integrator(void) const; @@ -57,13 +66,6 @@ public: void initialize(float initial_output = 0.0); float apply(float current_input); - Tracking_loop_filter(float update_interval, float noise_bandwidth, - int loop_order = 2, - bool include_last_integrator = false); - - Tracking_loop_filter(); - ~Tracking_loop_filter(); - private: // Store the last inputs and outputs: std::vector d_inputs; From 5d679a3eef73e221ce43e15fff98c71df7889d3a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 15:24:28 +0200 Subject: [PATCH 51/64] Do not use move when it is not needed --- src/core/system_parameters/gnss_satellite.cc | 6 +++--- src/core/system_parameters/gnss_signal.cc | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index c928ce069..8284baf9a 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -126,7 +126,7 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) // Move constructor Gnss_Satellite::Gnss_Satellite(const Gnss_Satellite& other) { - *this = std::move(other); + *this = other; } @@ -137,9 +137,9 @@ Gnss_Satellite& Gnss_Satellite::operator=(Gnss_Satellite&& other) { this->reset(); this->system = std::move(other.get_system()); - this->PRN = std::move(other.get_PRN()); + this->PRN = other.get_PRN(); this->block = std::move(other.get_block()); - this->rf_link = std::move(other.get_rf_link()); + this->rf_link = other.get_rf_link(); } return *this; } diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index 520d7242a..d1e838140 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -77,7 +77,7 @@ Gnss_Signal& Gnss_Signal::operator=(const Gnss_Signal& rhs) // Move constructor Gnss_Signal::Gnss_Signal(const Gnss_Signal& other) { - *this = std::move(other); + *this = other; } From 755dd7901fc01957fba2667c4cb0510677861e24 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 16:13:35 +0200 Subject: [PATCH 52/64] Add copy constructor, move constructor and move assignment operator. Fix memory leak --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 33 +++++++++++++++++++- src/core/monitor/serdes_gnss_synchro.h | 33 +++++++++++++++++++- 2 files changed, 64 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 490580818..a03e297db 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -48,7 +48,6 @@ public: // Verify that the version of the library that we linked against is // compatible with the version of the headers we compiled against. GOOGLE_PROTOBUF_VERIFY_VERSION; - monitor_.New(); } ~Serdes_Monitor_Pvt() @@ -56,6 +55,36 @@ public: // google::protobuf::ShutdownProtobufLibrary(); } + inline Serdes_Monitor_Pvt(Serdes_Monitor_Pvt&& other) //!< Copy constructor + { + this->monitor_ = other.monitor_; + } + + inline Serdes_Monitor_Pvt& operator=(const Serdes_Monitor_Pvt& rhs) //!< Copy assignment operator + { + // Only do assignment if RHS is a different object from this. + if (this != &rhs) + { + // Deallocate, allocate new space, copy values... + this->monitor_ = rhs.monitor_; + } + return *this; + } + + inline Serdes_Monitor_Pvt(const Serdes_Monitor_Pvt& other) //!< Move constructor + { + this->monitor_ = std::move(other.monitor_); + } + + inline Serdes_Monitor_Pvt& operator=(Serdes_Monitor_Pvt&& other) //!< Move assignment operator + { + if (this != &other) + { + this->monitor_ = std::move(other.monitor_); + } + return *this; + } + inline std::string createProtobuffer(const Monitor_Pvt& monitor) //!< Serialization into a string { monitor_.Clear(); @@ -91,6 +120,8 @@ public: monitor_.set_hdop(monitor.hdop); monitor_.set_vdop(monitor.vdop); + monitor_.CheckInitialized(); + monitor_.SerializeToString(&data); return data; } diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 0e63aed6c..39ececdab 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -50,7 +50,6 @@ public: // Verify that the version of the library that we linked against is // compatible with the version of the headers we compiled against. GOOGLE_PROTOBUF_VERIFY_VERSION; - observables.New(); } ~Serdes_Gnss_Synchro() @@ -58,6 +57,36 @@ public: google::protobuf::ShutdownProtobufLibrary(); } + inline Serdes_Gnss_Synchro(Serdes_Gnss_Synchro&& other) //!< Copy constructor + { + this->observables = other.observables; + } + + inline Serdes_Gnss_Synchro& operator=(const Serdes_Gnss_Synchro& rhs) //!< Copy assignment operator + { + // Only do assignment if RHS is a different object from this. + if (this != &rhs) + { + // Deallocate, allocate new space, copy values... + this->observables = rhs.observables; + } + return *this; + } + + inline Serdes_Gnss_Synchro(const Serdes_Gnss_Synchro& other) //!< Move constructor + { + this->observables = std::move(other.observables); + } + + inline Serdes_Gnss_Synchro& operator=(Serdes_Gnss_Synchro&& other) //!< Move assignment operator + { + if (this != &other) + { + this->observables = std::move(other.observables); + } + return *this; + } + inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { observables.Clear(); @@ -101,6 +130,8 @@ public: obs->set_rx_time(gs.RX_time); obs->set_flag_valid_pseudorange(gs.Flag_valid_pseudorange); obs->set_interp_tow_ms(gs.interp_TOW_ms); + + obs->CheckInitialized(); } observables.SerializeToString(&data); return data; From 76a14a86f017b4b6d70967856b671bff53a94147 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 16:14:33 +0200 Subject: [PATCH 53/64] Fix data race condition detected by Coverity Scan --- .../gnuradio_blocks/galileo_telemetry_decoder_gs.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index a94aebcf8..b73690f0e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -664,6 +664,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_CRC_error_counter = 0; d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + gr::thread::scoped_lock lock(d_setlock); d_last_valid_preamble = d_sample_counter; if (!d_flag_frame_sync) { From 0d3299f29b2a02b11d91c84cabe1cc6aa26e46f9 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 5 Jul 2019 20:30:34 +0200 Subject: [PATCH 54/64] Fix Beidou. Disabled symbol integration in tracking for Beidou and disabled experimental tracking carrier Doppler correction for all systems --- .../beidou_b1i_telemetry_decoder_gs.cc | 315 +++++++-------- .../beidou_b1i_telemetry_decoder_gs.h | 24 +- .../beidou_b3i_telemetry_decoder_gs.cc | 374 +++++++++--------- .../beidou_b3i_telemetry_decoder_gs.h | 21 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 67 ++-- src/algorithms/tracking/libs/dll_pll_conf.cc | 1 + src/algorithms/tracking/libs/dll_pll_conf.h | 1 + 7 files changed, 400 insertions(+), 403 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index b597504b3..57f8d5e7b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -29,10 +29,10 @@ * * ------------------------------------------------------------------------- */ + + #include "beidou_b1i_telemetry_decoder_gs.h" #include "Beidou_B1I.h" -#include "Beidou_DNAV.h" -#include "beidou_dnav_almanac.h" #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" @@ -57,6 +57,7 @@ beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) return beidou_b1i_telemetry_decoder_gs_sptr(new beidou_b1i_telemetry_decoder_gs(satellite, dump)); } + beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( const Gnss_Satellite &satellite, bool dump) : gr::block("beidou_b1i_telemetry_decoder_gs", @@ -74,38 +75,63 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite; - d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; + d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) + // Setting samples of secondary code + for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++) { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1') { - d_preamble_samples[i] = 1; + d_secondary_code_symbols[i] = 1; } else { - d_preamble_samples[i] = -1; + d_secondary_code_symbols[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + // Setting samples of preamble code + int32_t n = 0; + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + int32_t m = 0; + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } + else + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } + } + + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); - d_last_valid_preamble = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; d_preamble_index = 0; d_flag_frame_sync = false; - d_TOW_at_current_symbol_ms = 0U; + d_TOW_at_current_symbol_ms = 0; d_TOW_at_Preamble_ms = 0U; Flag_valid_word = false; d_CRC_error_counter = 0; @@ -118,6 +144,7 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -165,7 +192,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in void beidou_b1i_telemetry_decoder_gs::decode_word( int32_t word_counter, - const float *enc_word_symbols, + const double *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -205,7 +232,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_word( } -void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) +void beidou_b1i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -236,13 +263,11 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // 3. Check operation executed correctly if (d_nav.flag_crc_test == true) { - DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite; } else { - DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) @@ -297,59 +322,41 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); + d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = nullptr; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; // Setting samples of preamble code + int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - d_preamble_samples[i] = 1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = 1; + n++; + } } else { - d_preamble_samples[i] = -1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -1; + n++; + } } } - d_symbol_duration_ms = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); - } - else - { - // Clear values from previous declaration - volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_subframe_symbols); - //back to normal satellites - d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; - d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; - - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) - { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') - { - d_preamble_samples[i] = 1; - } - else - { - d_preamble_samples[i] = -1; - } - } - - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); } } @@ -380,15 +387,6 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel) } } -void beidou_b1i_telemetry_decoder_gs::reset() -{ - d_last_valid_preamble = d_sample_counter; - d_TOW_at_current_symbol_ms = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; - DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite; - return; -} int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -405,9 +403,10 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); + d_flag_preamble = false; - if (d_symbol_history.size() >= d_required_symbols) + if (d_symbol_history.size() > d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -422,6 +421,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ } } } + //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -429,7 +429,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - DLOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; + LOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -443,54 +443,9 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; + LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; d_preamble_index = d_sample_counter; //record the preamble sample stamp - - d_stat = 2; - - // ******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) //normal PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = d_symbol_history.at(i); - } - } - else // 180 deg. inverted carrier phase PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = -d_symbol_history.at(i); - } - } - - // call the decoder - decode_subframe(d_subframe_symbols); - - if (d_nav.flag_crc_test == true) - { - d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) - if (!d_flag_frame_sync) - { - d_flag_frame_sync = true; - DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " << this->d_satellite; - } - } - else - { - d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp - if (d_CRC_error_counter > CRC_ERROR_LIMIT) - { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; - d_flag_frame_sync = false; - d_stat = 0; - flag_SOW_set = false; - } - } } else { @@ -509,16 +464,50 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ // ******* SAMPLES TO SYMBOLS ******* if (corr_value > 0) //normal PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } } } else // 180 deg. inverted carrier phase PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = -d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } } } @@ -542,7 +531,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; + LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -550,6 +539,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ } } } + // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) @@ -557,67 +547,52 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - //check TOW update consistency - uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - //compute new TOW - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * BEIDOU_B1I_CODE_PERIOD_MS); flag_SOW_set = true; d_nav.flag_new_SOW_available = false; + } + else // if there is not a new preamble, we define the TOW of the current symbol + { + d_TOW_at_current_symbol_ms += static_cast(BEIDOU_B1I_CODE_PERIOD_MS); + } - if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) - { - LOG(INFO) << "Warning: BEIDOU B1I TOW update in ch " << d_channel - << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; - d_TOW_at_current_symbol_ms = 0; - d_flag_valid_word = false; - } - else - { - d_last_valid_preamble = d_sample_counter; - d_flag_valid_word = true; - } + if (d_flag_frame_sync == true and flag_SOW_set == true) + { + current_symbol.Flag_valid_word = true; } else { - if (d_flag_valid_word) - { - d_TOW_at_current_symbol_ms += d_symbol_duration_ms; - if (current_symbol.Flag_valid_symbol_output == false) - { - d_flag_valid_word = false; - } - } + current_symbol.Flag_valid_word = false; } - if (d_flag_valid_word == true) + current_symbol.PRN = this->d_satellite.get_PRN(); + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + + if (d_dump == true) { - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - current_symbol.Flag_valid_word = d_flag_valid_word; - - if (d_dump == true) + // MULTIPLEXED FILE RECORDING - Record results to file + try { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); - } + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = d_nav.d_SOW; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = static_cast(d_required_symbols); + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - return 1; } - return 0; + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + + return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 9485008fd..3a7b8debf 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -39,7 +39,7 @@ #include #include // for boost::shared_ptr #include // for block -#include // for gr_vector_const_void_star +#include // for gr_vector_const_void_star #include #include #include @@ -62,8 +62,10 @@ public: ~beidou_b1i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - void reset(); - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ @@ -75,17 +77,19 @@ private: beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); beidou_b1i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(float *symbols); - void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols); + void decode_subframe(double *symbols); + void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; + int32_t *d_secondary_code_symbols; + uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - float *d_subframe_symbols; + double *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data @@ -103,16 +107,12 @@ private: //!< Navigation Message variable Beidou_Dnav_Navigation_Message d_nav; - // Values to populate gnss synchronization structure - uint32_t d_symbol_duration_ms; + //!< Values to populate gnss synchronization structure uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; - uint64_t d_last_valid_preamble; - bool d_flag_valid_word; - bool d_sent_tlm_failed_msg; bool Flag_valid_word; - // Satellite Information and logging capacity + //!< Satellite Information and logging capacity Gnss_Satellite d_satellite; int32_t d_channel; bool d_dump; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 419aff764..01530cd32 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -53,7 +53,8 @@ beidou_b3i_telemetry_decoder_gs_sptr beidou_b3i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) { - return beidou_b3i_telemetry_decoder_gs_sptr(new beidou_b3i_telemetry_decoder_gs(satellite, dump)); + return beidou_b3i_telemetry_decoder_gs_sptr( + new beidou_b3i_telemetry_decoder_gs(satellite, dump)); } @@ -69,37 +70,74 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); + // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " << this->d_satellite; + LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " + << this->d_satellite; - d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; + d_samples_per_symbol = + (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / + BEIDOU_D1NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_samples_per_preamble = + BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = static_cast( + volk_gnsssdr_malloc(BEIDOU_B3I_SECONDARY_CODE_LENGTH * sizeof(int32_t), + volk_gnsssdr_get_alignment())); + d_preamble_samples = static_cast(volk_gnsssdr_malloc( + d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) + // Setting samples of secondary code + for (int32_t i = 0; i < BEIDOU_B3I_SECONDARY_CODE_LENGTH; i++) { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + if (BEIDOU_B3I_SECONDARY_CODE.at(i) == '1') { - d_preamble_samples[i] = 1; + d_secondary_code_symbols[i] = 1; } else { - d_preamble_samples[i] = -1; + d_secondary_code_symbols[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + // Setting samples of preamble code + int32_t n = 0; + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + int32_t m = 0; + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } + else + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } + } + + d_subframe_symbols = static_cast( + volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), + volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); - d_last_valid_preamble = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; @@ -118,6 +156,7 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -128,7 +167,8 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() } catch (const std::exception &ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + LOG(WARNING) << "Exception in destructor closing the dump file " + << ex.what(); } } } @@ -165,8 +205,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, void beidou_b3i_telemetry_decoder_gs::decode_word( - int32_t word_counter, - const float *enc_word_symbols, + int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -175,7 +214,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t j = 0; j < 30; j++) { - dec_word_symbols[j] = static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); + dec_word_symbols[j] = + static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); } } else @@ -184,7 +224,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t c = 0; c < 15; c++) { - bitsbch[r * 15 + c] = static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); + bitsbch[r * 15 + c] = + static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); } } @@ -206,7 +247,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( } -void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) +void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -315,62 +356,50 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite( { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); - + d_samples_per_symbol = + (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / + BEIDOU_D2NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), - volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_samples_per_preamble = + BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = nullptr; + d_preamble_samples = static_cast( + volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), + volk_gnsssdr_get_alignment())); + d_preamble_period_samples = + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; // Setting samples of preamble code + int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - d_preamble_samples[i] = 1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = 1; + n++; + } } else { - d_preamble_samples[i] = -1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -1; + n++; + } } } - d_symbol_duration_ms = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; - d_subframe_symbols = static_cast(volk_gnsssdr_malloc( - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), + + d_subframe_symbols = static_cast(volk_gnsssdr_malloc( + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); - } - else - { - // Clear values from previous declaration - volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_subframe_symbols); - //back to normal satellites - d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; - d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; - - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) - { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') - { - d_preamble_samples[i] = 1; - } - else - { - d_preamble_samples[i] = -1; - } - } - - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); } } @@ -405,15 +434,6 @@ void beidou_b3i_telemetry_decoder_gs::set_channel(int32_t channel) } } -void beidou_b3i_telemetry_decoder_gs::reset() -{ - d_last_valid_preamble = d_sample_counter; - d_TOW_at_current_symbol_ms = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; - DLOG(INFO) << "Beidou B3I Telemetry decoder reset for satellite " << d_satellite; - return; -} int beidou_b3i_telemetry_decoder_gs::general_work( int noutput_items __attribute__((unused)), @@ -434,9 +454,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); + d_flag_preamble = false; - if (d_symbol_history.size() >= d_required_symbols) + if (d_symbol_history.size() > d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -451,6 +472,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } + //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -458,7 +480,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - DLOG(INFO) << "Preamble detection for BEIDOU B3I SAT " << this->d_satellite; + LOG(INFO) << "Preamble detection for BEIDOU B3I SAT " + << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -472,55 +495,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " - << this->d_satellite; + LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " + << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp d_stat = 2; - - // ******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) //normal PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = d_symbol_history.at(i); - } - } - else // 180 deg. inverted carrier phase PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = -d_symbol_history.at(i); - } - } - - // call the decoder - decode_subframe(d_subframe_symbols); - - if (d_nav.flag_crc_test == true) - { - d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) - if (!d_flag_frame_sync) - { - d_flag_frame_sync = true; - DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " - << this->d_satellite; - } - } - else - { - d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp - if (d_CRC_error_counter > CRC_ERROR_LIMIT) - { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " - << this->d_satellite; - d_flag_frame_sync = false; - d_stat = 0; - flag_SOW_set = false; - } - } } else { @@ -535,21 +513,62 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } else if (d_stat == 2) // preamble acquired { - if (d_sample_counter == d_preamble_index + static_cast(d_preamble_period_samples)) + if (d_sample_counter == + d_preamble_index + static_cast(d_preamble_period_samples)) { - // ******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) //normal PLL lock + //******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) // normal PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += + d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += + static_cast(d_secondary_code_symbols[k]) * + d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } } } else // 180 deg. inverted carrier phase PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = -d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= + d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= + static_cast(d_secondary_code_symbols[k]) * + d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } } } @@ -559,8 +578,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (d_nav.flag_crc_test == true) { d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + d_flag_preamble = true; // valid preamble indicator (initialized to + // false every work()) + d_preamble_index = + d_sample_counter; // record the preamble sample stamp (t_P) if (!d_flag_frame_sync) { d_flag_frame_sync = true; @@ -574,8 +595,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " - << this->d_satellite; + LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " + << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -583,6 +604,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } + // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) @@ -590,67 +612,55 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - //check TOW update consistency - uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - //compute new TOW - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; + d_TOW_at_current_symbol_ms = + d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * + BEIDOU_B3I_CODE_PERIOD_MS); flag_SOW_set = true; d_nav.flag_new_SOW_available = false; + } + else // if there is not a new preamble, we define the TOW of the current + // symbol + { + d_TOW_at_current_symbol_ms += + static_cast(BEIDOU_B3I_CODE_PERIOD_MS); + } - if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) - { - LOG(INFO) << "Warning: BEIDOU B3I TOW update in ch " << d_channel - << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; - - d_TOW_at_current_symbol_ms = 0; - d_flag_valid_word = false; - } - else - { - d_last_valid_preamble = d_sample_counter; - d_flag_valid_word = true; - } + if (d_flag_frame_sync == true and flag_SOW_set == true) + { + current_symbol.Flag_valid_word = true; } else { - if (d_flag_valid_word) - { - d_TOW_at_current_symbol_ms += d_symbol_duration_ms; - if (current_symbol.Flag_valid_symbol_output == false) - { - d_flag_valid_word = false; - } - } + current_symbol.Flag_valid_word = false; } - if (d_flag_valid_word == true) + current_symbol.PRN = this->d_satellite.get_PRN(); + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + + if (d_dump == true) { - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - current_symbol.Flag_valid_word = d_flag_valid_word; - - if (d_dump == true) + // MULTIPLEXED FILE RECORDING - Record results to file + try { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); - } + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = d_nav.d_SOW; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = static_cast(d_required_symbols); + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - return 1; } - return 0; + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + + return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h index d08f8388e..54bac3adb 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h @@ -60,8 +60,10 @@ public: ~beidou_b3i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - void reset(); - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ @@ -75,24 +77,27 @@ private: bool dump); beidou_b3i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(float *symbols); - void decode_word(int32_t word_counter, const float *enc_word_symbols, + void decode_subframe(double *symbols); + void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; + int32_t *d_secondary_code_symbols; + uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - float *d_subframe_symbols; + double *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data boost::circular_buffer d_symbol_history; // Variables for internal functionality - uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed + uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) + // indicating number of samples processed uint64_t d_preamble_index; // Index of sample number where preamble was found uint32_t d_stat; // Status of decoder bool d_flag_frame_sync; // Indicate when a frame sync is achieved @@ -104,12 +109,8 @@ private: Beidou_Dnav_Navigation_Message d_nav; // Values to populate gnss synchronization structure - uint32_t d_symbol_duration_ms; uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; - uint64_t d_last_valid_preamble; - bool d_flag_valid_word; - bool d_sent_tlm_failed_msg; bool Flag_valid_word; // Satellite Information and logging capacity diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 2f46f0f95..940b43284 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -286,16 +286,17 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = BEIDOU_B1I_CODE_PERIOD; d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ; d_code_length_chips = static_cast(BEIDOU_B1I_CODE_LENGTH_CHIPS); - d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; + //d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; // synchronize and remove data secondary code d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); - d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); - d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); } else if (signal_type == "B3") { @@ -304,15 +305,16 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = BEIDOU_B3I_CODE_PERIOD; d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ; d_code_length_chips = static_cast(BEIDOU_B3I_CODE_LENGTH_CHIPS); - d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; + //d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); - d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); - d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); } else { @@ -649,7 +651,8 @@ void dll_pll_veml_tracking::start_tracking() // GEO Satellites use different secondary code if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { - d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; + //d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_secondary = false; @@ -662,16 +665,17 @@ void dll_pll_veml_tracking::start_tracking() } else { - d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; + //d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; // synchronize and remove data secondary code d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); - d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); - d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -682,7 +686,8 @@ void dll_pll_veml_tracking::start_tracking() // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { - d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; + //d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_secondary = false; @@ -695,16 +700,17 @@ void dll_pll_veml_tracking::start_tracking() } else { - d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; + //d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; // synchronize and remove data secondary code d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); - d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); - d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -1002,21 +1008,24 @@ void dll_pll_veml_tracking::run_dll_pll() d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler - if (d_pull_in_transitory == false and d_corrected_doppler == false) + if (trk_parameters.enable_doppler_correction == true) { - d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); - - if (d_dll_filt_history.full()) + if (d_pull_in_transitory == false and d_corrected_doppler == false) { - float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); - if (fabs(avg_code_error_chips_s) > 0.10) + d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + + if (d_dll_filt_history.full()) { - float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); - LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); - d_corrected_doppler = true; + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); + LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); + d_corrected_doppler = true; + } + d_dll_filt_history.clear(); } - d_dll_filt_history.clear(); } } } diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index f31bccc4e..81ecb99b6 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -69,6 +69,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() max_carrier_lock_fail = FLAGS_max_carrier_lock_fail; max_code_lock_fail = FLAGS_max_lock_fail; carrier_lock_th = FLAGS_carrier_lock_th; + enable_doppler_correction = false; track_pilot = false; system = 'G'; char sig_[3] = "1C"; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index d84e8f204..04f42f055 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -75,6 +75,7 @@ public: uint32_t smoother_length; double carrier_lock_th; bool track_pilot; + bool enable_doppler_correction; char system; char signal[3]{}; From 95ece1e6d4f4da4cb6b14c95b4b89b1d77c621f1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 Jul 2019 12:34:27 +0200 Subject: [PATCH 55/64] Simplify move assignment operator --- .../tracking/libs/exponential_smoother.cc | 16 +--------------- .../tracking/libs/exponential_smoother.h | 8 ++++---- .../tracking/libs/tracking_loop_filter.cc | 17 +---------------- 3 files changed, 6 insertions(+), 35 deletions(-) diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 186882d06..5c531fcab 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -52,21 +52,7 @@ Exponential_Smoother::~Exponential_Smoother() = default; // Move assignment operator -Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) -{ - if (this != &other) - { - this->alpha_ = other.alpha_; - this->old_value_ = other.old_value_; - this->one_minus_alpha_ = 1.0 - other.alpha_; - this->samples_for_initialization_ = other.samples_for_initialization_; - this->initializing_ = other.initializing_; - this->init_counter_ = other.init_counter_; - this->min_value_ = other.min_value_; - this->offset_ = other.offset_; - } - return *this; -} +Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) = default; void Exponential_Smoother::set_alpha(float alpha) diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index dc7f222d4..e0cc86af9 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -47,10 +47,10 @@ class Exponential_Smoother { public: - Exponential_Smoother(); //!< Constructor - ~Exponential_Smoother(); //!< Destructor - void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001 - void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200 + Exponential_Smoother(); //!< Constructor + ~Exponential_Smoother(); //!< Destructor + void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001 + void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200 void reset(); void set_min_value(float value); void set_offset(float offset); diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index d2b581d2f..821efa67c 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -72,22 +72,7 @@ Tracking_loop_filter::~Tracking_loop_filter() = default; // Move assignment operator -Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) -{ - if (this != &other) - { - this->d_inputs = other.d_inputs; - this->d_outputs = other.d_outputs; - this->d_input_coefficients = other.d_input_coefficients; - this->d_output_coefficients = other.d_output_coefficients; - this->d_loop_order = other.d_loop_order; - this->d_current_index = other.d_current_index; - this->d_include_last_integrator = other.d_include_last_integrator; - this->d_noise_bandwidth = other.d_noise_bandwidth; - this->d_update_interval = other.d_update_interval; - } - return *this; -} +Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) = default; float Tracking_loop_filter::apply(float current_input) From 5f1779c15bf1624b608cdd257acad0062d2caf3c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 Jul 2019 13:55:19 +0200 Subject: [PATCH 56/64] Remove test for speed --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 2 -- src/core/monitor/serdes_gnss_synchro.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index a03e297db..3294a5fc1 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -120,8 +120,6 @@ public: monitor_.set_hdop(monitor.hdop); monitor_.set_vdop(monitor.vdop); - monitor_.CheckInitialized(); - monitor_.SerializeToString(&data); return data; } diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 39ececdab..c90e93282 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -130,8 +130,6 @@ public: obs->set_rx_time(gs.RX_time); obs->set_flag_valid_pseudorange(gs.Flag_valid_pseudorange); obs->set_interp_tow_ms(gs.interp_TOW_ms); - - obs->CheckInitialized(); } observables.SerializeToString(&data); return data; From cf8f4af52768467a2813e6816dfc930ce77e1381 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 18:41:50 +0200 Subject: [PATCH 57/64] Try to avoid non-default move & copy constructors --- src/core/system_parameters/gnss_satellite.cc | 24 +++++--------------- src/core/system_parameters/gnss_satellite.h | 14 ++++++------ src/core/system_parameters/gnss_signal.cc | 6 ++--- src/core/system_parameters/gnss_signal.h | 10 ++++---- 4 files changed, 20 insertions(+), 34 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 8284baf9a..089a0877b 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -47,17 +47,8 @@ Gnss_Satellite::Gnss_Satellite(const std::string& system_, uint32_t PRN_) } -Gnss_Satellite::~Gnss_Satellite() = default; - - void Gnss_Satellite::reset() { - system_set = {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"}; - satelliteSystem["GPS"] = "G"; - satelliteSystem["Glonass"] = "R"; - satelliteSystem["SBAS"] = "S"; - satelliteSystem["Galileo"] = "E"; - satelliteSystem["Beidou"] = "C"; PRN = 0; system = std::string(""); block = std::string(""); @@ -102,7 +93,7 @@ bool operator==(const Gnss_Satellite& sat1, const Gnss_Satellite& sat2) // Copy constructor Gnss_Satellite::Gnss_Satellite(Gnss_Satellite&& other) { - *this = std::move(other); + *this = other; } @@ -112,12 +103,10 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) // Only do assignment if RHS is a different object from this. if (this != &rhs) { - // Deallocate, allocate new space, copy values... - this->reset(); - this->set_system(rhs.get_system()); - this->set_PRN(rhs.get_PRN()); - this->set_block(rhs.get_system(), rhs.get_PRN()); - this->set_rf_link(rhs.get_rf_link()); + this->system = rhs.system; + this->PRN = rhs.PRN; + this->block = rhs.block; + this->rf_link = rhs.rf_link; } return *this; } @@ -126,7 +115,7 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) // Move constructor Gnss_Satellite::Gnss_Satellite(const Gnss_Satellite& other) { - *this = other; + *this = std::move(other); } @@ -135,7 +124,6 @@ Gnss_Satellite& Gnss_Satellite::operator=(Gnss_Satellite&& other) { if (this != &other) { - this->reset(); this->system = std::move(other.get_system()); this->PRN = other.get_PRN(); this->block = std::move(other.get_block()); diff --git a/src/core/system_parameters/gnss_satellite.h b/src/core/system_parameters/gnss_satellite.h index 79fe9dead..b00c4c561 100644 --- a/src/core/system_parameters/gnss_satellite.h +++ b/src/core/system_parameters/gnss_satellite.h @@ -50,7 +50,7 @@ class Gnss_Satellite public: Gnss_Satellite(); //!< Default Constructor. Gnss_Satellite(const std::string& system_, uint32_t PRN_); //!< Concrete GNSS satellite Constructor. - ~Gnss_Satellite(); //!< Default Destructor. + ~Gnss_Satellite() = default; //!< Default Destructor. void update_PRN(uint32_t PRN); //!< Updates the PRN Number when information is decoded, only applies to GLONASS GNAV messages uint32_t get_PRN() const; //!< Gets satellite's PRN int32_t get_rf_link() const; //!< Gets the satellite's rf link @@ -68,15 +68,15 @@ public: Gnss_Satellite& operator=(Gnss_Satellite&& other); //!< Move assignment operator private: - uint32_t PRN; - std::string system; - std::map satelliteSystem; - std::string block; - int32_t rf_link; + uint32_t PRN{}; + int32_t rf_link{}; + std::string system{}; + std::string block{}; + const std::set system_set = {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"}; + const std::map satelliteSystem = {{"GPS", "G"}, {"Glonass", "R"}, {"SBAS", "S"}, {"Galileo", "E"}, {"Beidou", "C"}}; void set_system(const std::string& system); // Sets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}. void set_PRN(uint32_t PRN); // Sets satellite's PRN void set_block(const std::string& system_, uint32_t PRN_); - std::set system_set; // = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}; void reset(); void set_rf_link(int32_t rf_link_); }; diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index d1e838140..c73382d1c 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -50,9 +50,7 @@ Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& si } -Gnss_Signal::~Gnss_Signal() = default; - - +/* // Copy constructor Gnss_Signal::Gnss_Signal(Gnss_Signal&& other) { @@ -91,7 +89,7 @@ Gnss_Signal& Gnss_Signal::operator=(Gnss_Signal&& other) } return *this; } - +*/ std::string Gnss_Signal::get_signal_str() const { diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 92051444a..62f69b5ee 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -47,20 +47,20 @@ public: Gnss_Signal(); Gnss_Signal(const std::string& signal_); Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_); - ~Gnss_Signal(); + ~Gnss_Signal() = default; std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "L5" for GPS L5, "1G" for GLONASS L1 C/A, "1B" for Galileo E1B, "5X" for Galileo E5a. Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing - Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor + /*Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor Gnss_Signal& operator=(const Gnss_Signal&); //!< Copy assignment operator Gnss_Signal(const Gnss_Signal& other); //!< Move constructor - Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator + Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator */ private: - Gnss_Satellite satellite; - std::string signal; + Gnss_Satellite satellite{}; + std::string signal{}; }; #endif From 29a910df300bdbae55bb451ccb7295a4f643a261 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 19:56:23 +0200 Subject: [PATCH 58/64] Simplify initializations --- .../PVT/libs/monitor_pvt_udp_sink.cc | 28 ----------- .../PVT/libs/monitor_pvt_udp_sink.h | 2 +- src/algorithms/PVT/libs/rtklib_solver.cc | 46 ++----------------- src/algorithms/PVT/libs/rtklib_solver.h | 10 ++-- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 5 +- src/core/monitor/serdes_gnss_synchro.h | 6 +-- src/core/receiver/file_configuration.h | 2 +- src/core/system_parameters/gnss_signal.cc | 41 ----------------- src/core/system_parameters/gnss_signal.h | 4 -- 9 files changed, 17 insertions(+), 127 deletions(-) diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc index 0e962207b..2796f13da 100644 --- a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc @@ -42,34 +42,6 @@ Monitor_Pvt_Udp_Sink::Monitor_Pvt_Udp_Sink(const std::vector& addre boost::asio::ip::udp::endpoint endpoint(boost::asio::ip::address::from_string(address, error), port); endpoints.push_back(endpoint); } - monitor_pvt.TOW_at_current_symbol_ms = 0U; - monitor_pvt.week = 0U; - monitor_pvt.RX_time = 0.0; - monitor_pvt.user_clk_offset = 0.0; - monitor_pvt.pos_x = 0.0; - monitor_pvt.pos_y = 0.0; - monitor_pvt.pos_z = 0.0; - monitor_pvt.vel_x = 0.0; - monitor_pvt.vel_y = 0.0; - monitor_pvt.vel_z = 0.0; - monitor_pvt.cov_xx = 0.0; - monitor_pvt.cov_yy = 0.0; - monitor_pvt.cov_zz = 0.0; - monitor_pvt.cov_xy = 0.0; - monitor_pvt.cov_yz = 0.0; - monitor_pvt.cov_zx = 0.0; - monitor_pvt.latitude = 0.0; - monitor_pvt.longitude = 0.0; - monitor_pvt.height = 0.0; - monitor_pvt.valid_sats = 0; - monitor_pvt.solution_status = 0; - monitor_pvt.solution_type = 0; - monitor_pvt.AR_ratio_factor = 0.0; - monitor_pvt.AR_ratio_threshold = 0.0; - monitor_pvt.gdop = 0.0; - monitor_pvt.pdop = 0.0; - monitor_pvt.hdop = 0.0; - monitor_pvt.vdop = 0.0; use_protobuf = protobuf_enabled; if (use_protobuf) diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h index 85a612358..f1f9146f4 100644 --- a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h @@ -53,7 +53,7 @@ private: boost::asio::ip::udp::socket socket; boost::system::error_code error; std::vector endpoints; - Monitor_Pvt monitor_pvt; + Monitor_Pvt monitor_pvt{}; Serdes_Monitor_Pvt serdes; bool use_protobuf; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index ebb359b0f..0f6e84b06 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -77,16 +77,7 @@ Rtklib_Solver::Rtklib_Solver(int nchannels, std::string dump_filename, bool flag count_valid_position = 0; this->set_averaging_flag(false); rtk_ = rtk; - for (double &i : dop_) - { - i = 0.0; - } - pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0}; - ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}}; - for (auto &i : pvt_ssat) - { - i = ssat0; - } + // ############# ENABLE DATA FILE LOG ################# if (d_flag_dump_enabled == true) { @@ -104,35 +95,6 @@ Rtklib_Solver::Rtklib_Solver(int nchannels, std::string dump_filename, bool flag } } } - // PVT MONITOR - monitor_pvt.TOW_at_current_symbol_ms = 0U; - monitor_pvt.week = 0U; - monitor_pvt.RX_time = 0.0; - monitor_pvt.user_clk_offset = 0.0; - monitor_pvt.pos_x = 0.0; - monitor_pvt.pos_y = 0.0; - monitor_pvt.pos_z = 0.0; - monitor_pvt.vel_x = 0.0; - monitor_pvt.vel_y = 0.0; - monitor_pvt.vel_z = 0.0; - monitor_pvt.cov_xx = 0.0; - monitor_pvt.cov_yy = 0.0; - monitor_pvt.cov_zz = 0.0; - monitor_pvt.cov_xy = 0.0; - monitor_pvt.cov_yz = 0.0; - monitor_pvt.cov_zx = 0.0; - monitor_pvt.latitude = 0.0; - monitor_pvt.longitude = 0.0; - monitor_pvt.height = 0.0; - monitor_pvt.valid_sats = 0; - monitor_pvt.solution_status = 0; - monitor_pvt.solution_type = 0; - monitor_pvt.AR_ratio_factor = 0.0; - monitor_pvt.AR_ratio_threshold = 0.0; - monitor_pvt.gdop = 0.0; - monitor_pvt.pdop = 0.0; - monitor_pvt.hdop = 0.0; - monitor_pvt.vdop = 0.0; } bool Rtklib_Solver::save_matfile() @@ -450,9 +412,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ int valid_obs = 0; // valid observations counter int glo_valid_obs = 0; // GLONASS L1/L2 valid observations counter - std::array obs_data; - std::array eph_data; - std::array geph_data; + std::array obs_data{}; + std::array eph_data{}; + std::array geph_data{}; // Workaround for NAV/CNAV clash problem bool gps_dual_band = false; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 8e1b94cc7..128d8bebf 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -94,8 +94,8 @@ public: bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); - sol_t pvt_sol; - std::array pvt_ssat; + sol_t pvt_sol{}; + std::array pvt_ssat{}; double get_hdop() const; double get_vdop() const; double get_pdop() const; @@ -129,15 +129,15 @@ public: int count_valid_position; private: - rtk_t rtk_; + rtk_t rtk_{}; std::string d_dump_filename; std::ofstream d_dump_file; bool save_matfile(); bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; int d_nchannels; // Number of available channels for positioning - std::array dop_; - Monitor_Pvt monitor_pvt; + std::array dop_{}; + Monitor_Pvt monitor_pvt{}; }; #endif diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 3294a5fc1..2e5b95a17 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -55,6 +55,7 @@ public: // google::protobuf::ShutdownProtobufLibrary(); } + /* inline Serdes_Monitor_Pvt(Serdes_Monitor_Pvt&& other) //!< Copy constructor { this->monitor_ = other.monitor_; @@ -83,7 +84,7 @@ public: this->monitor_ = std::move(other.monitor_); } return *this; - } + }*/ inline std::string createProtobuffer(const Monitor_Pvt& monitor) //!< Serialization into a string { @@ -161,7 +162,7 @@ public: } private: - gnss_sdr::MonitorPvt monitor_; + gnss_sdr::MonitorPvt monitor_{}; }; #endif // GNSS_SDR_SERDES_MONITOR_PVT_H_ diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index c90e93282..935c292de 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -56,7 +56,7 @@ public: { google::protobuf::ShutdownProtobufLibrary(); } - + /* inline Serdes_Gnss_Synchro(Serdes_Gnss_Synchro&& other) //!< Copy constructor { this->observables = other.observables; @@ -85,7 +85,7 @@ public: this->observables = std::move(other.observables); } return *this; - } + }*/ inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { @@ -179,7 +179,7 @@ public: } private: - gnss_sdr::Observables observables; + gnss_sdr::Observables observables{}; }; #endif // GNSS_SDR_SERDES_GNSS_SYNCHRO_H_ diff --git a/src/core/receiver/file_configuration.h b/src/core/receiver/file_configuration.h index 0e2ccb53e..c4d480d0b 100644 --- a/src/core/receiver/file_configuration.h +++ b/src/core/receiver/file_configuration.h @@ -79,7 +79,7 @@ private: std::shared_ptr ini_reader_; std::shared_ptr overrided_; std::unique_ptr converter_; - int error_; + int error_{}; }; #endif /*GNSS_SDR_FILE_CONFIGURATION_H_*/ diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index c73382d1c..71b0603fc 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -50,47 +50,6 @@ Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& si } -/* -// Copy constructor -Gnss_Signal::Gnss_Signal(Gnss_Signal&& other) -{ - *this = std::move(other); -} - - -// Copy assignment operator -Gnss_Signal& Gnss_Signal::operator=(const Gnss_Signal& rhs) -{ - // Only do assignment if RHS is a different object from this. - if (this != &rhs) - { - // Deallocate, allocate new space, copy values... - this->satellite = rhs.get_satellite(); - this->signal = rhs.get_signal_str(); - } - return *this; -} - - -// Move constructor -Gnss_Signal::Gnss_Signal(const Gnss_Signal& other) -{ - *this = other; -} - - -// Move assignment operator -Gnss_Signal& Gnss_Signal::operator=(Gnss_Signal&& other) -{ - if (this != &other) - { - this->satellite = std::move(other.get_satellite()); - this->signal = std::move(other.get_signal_str()); - } - return *this; -} -*/ - std::string Gnss_Signal::get_signal_str() const { return this->signal; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 62f69b5ee..767a12a0c 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -54,10 +54,6 @@ public: friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing - /*Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor - Gnss_Signal& operator=(const Gnss_Signal&); //!< Copy assignment operator - Gnss_Signal(const Gnss_Signal& other); //!< Move constructor - Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator */ private: Gnss_Satellite satellite{}; std::string signal{}; From 6cc1de71185c11305b7e655d99d0e79ab2aeb6bb Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 21:44:58 +0200 Subject: [PATCH 59/64] File cleaning --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 10 ++------- .../gnuradio_blocks/beamformer.cc | 21 +------------------ .../input_filter/gnuradio_blocks/beamformer.h | 8 +++++-- .../tracking/libs/exponential_smoother.cc | 7 ------- .../tracking/libs/exponential_smoother.h | 5 +++-- .../tracking/libs/tracking_loop_filter.cc | 8 +------ .../tracking/libs/tracking_loop_filter.h | 5 +++-- src/core/monitor/serdes_gnss_synchro.h | 11 +++------- 8 files changed, 19 insertions(+), 56 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 2e5b95a17..1b5ca11ca 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -55,7 +55,6 @@ public: // google::protobuf::ShutdownProtobufLibrary(); } - /* inline Serdes_Monitor_Pvt(Serdes_Monitor_Pvt&& other) //!< Copy constructor { this->monitor_ = other.monitor_; @@ -63,12 +62,7 @@ public: inline Serdes_Monitor_Pvt& operator=(const Serdes_Monitor_Pvt& rhs) //!< Copy assignment operator { - // Only do assignment if RHS is a different object from this. - if (this != &rhs) - { - // Deallocate, allocate new space, copy values... - this->monitor_ = rhs.monitor_; - } + this->monitor_ = rhs.monitor_; return *this; } @@ -84,7 +78,7 @@ public: this->monitor_ = std::move(other.monitor_); } return *this; - }*/ + } inline std::string createProtobuffer(const Monitor_Pvt& monitor) //!< Serialization into a string { diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc index 9656f57d0..165890e66 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc @@ -31,11 +31,8 @@ #include "beamformer.h" #include -#include -#define GNSS_SDR_BEAMFORMER_CHANNELS 8 - beamformer_sptr make_beamformer_sptr() { return beamformer_sptr(new beamformer()); @@ -47,22 +44,6 @@ beamformer::beamformer() gr::io_signature::make(GNSS_SDR_BEAMFORMER_CHANNELS, GNSS_SDR_BEAMFORMER_CHANNELS, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(gr_complex))) { - //initialize weight vector - - if (posix_memalign(reinterpret_cast(&weight_vector), 16, GNSS_SDR_BEAMFORMER_CHANNELS * sizeof(gr_complex)) == 0) - { - }; - - for (int i = 0; i < GNSS_SDR_BEAMFORMER_CHANNELS; i++) - { - weight_vector[i] = gr_complex(1, 0); - } -} - - -beamformer::~beamformer() -{ - free(weight_vector); } @@ -86,7 +67,7 @@ int beamformer::work(int noutput_items, gr_vector_const_void_star &input_items, for (int n = 0; n < noutput_items; n++) { sum = gr_complex(0, 0); - for (int i = 0; i < GNSS_SDR_BEAMFORMER_CHANNELS; i++) + for (int i = 0; i < weight_vector.size(); i++) { sum = sum + (reinterpret_cast(input_items[i]))[n] * weight_vector[i]; } diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h index b9cdd247a..1dcf76ef5 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h @@ -32,26 +32,30 @@ #define GNSS_SDR_BEAMFORMER_H #include +#include class beamformer; + using beamformer_sptr = boost::shared_ptr; beamformer_sptr make_beamformer_sptr(); +const int GNSS_SDR_BEAMFORMER_CHANNELS = 8; + /*! * \brief This class implements a real-time software-defined spatial filter using the CTTC GNSS experimental antenna array input and a set of dynamically reloadable weights */ class beamformer : public gr::sync_block { public: - ~beamformer(); + ~beamformer() = default; int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); private: friend beamformer_sptr make_beamformer_sptr(); beamformer(); - gr_complex *weight_vector; + std::vector weight_vector = std::vector(GNSS_SDR_BEAMFORMER_CHANNELS, gr_complex(1.0, 0.0)); }; #endif diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 5c531fcab..aa83a494e 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -48,13 +48,6 @@ Exponential_Smoother::Exponential_Smoother() } -Exponential_Smoother::~Exponential_Smoother() = default; - - -// Move assignment operator -Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) = default; - - void Exponential_Smoother::set_alpha(float alpha) { alpha_ = alpha; diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index e0cc86af9..7b597e08b 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -48,7 +48,7 @@ class Exponential_Smoother { public: Exponential_Smoother(); //!< Constructor - ~Exponential_Smoother(); //!< Destructor + ~Exponential_Smoother() = default; //!< Destructor void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001 void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200 void reset(); @@ -56,7 +56,8 @@ public: void set_offset(float offset); float smooth(float raw); double smooth(double raw); - Exponential_Smoother& operator=(Exponential_Smoother&& other); //!< Move assignment operator + Exponential_Smoother(Exponential_Smoother&&) = default; //!< Move operator + Exponential_Smoother& operator=(Exponential_Smoother&& /*other*/) = default; //!< Move assignment operator private: float alpha_; // takes value 0.0001 if not set int samples_for_initialization_; diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index 821efa67c..c01cca5f6 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -68,13 +68,6 @@ Tracking_loop_filter::Tracking_loop_filter() } -Tracking_loop_filter::~Tracking_loop_filter() = default; - - -// Move assignment operator -Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) = default; - - float Tracking_loop_filter::apply(float current_input) { // Now apply the filter coefficients: @@ -238,6 +231,7 @@ void Tracking_loop_filter::set_update_interval(float update_interval) update_coefficients(); } + float Tracking_loop_filter::get_update_interval(void) const { return d_update_interval; diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index 204b19825..b7dac4280 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -45,13 +45,14 @@ class Tracking_loop_filter { public: Tracking_loop_filter(); - ~Tracking_loop_filter(); + ~Tracking_loop_filter() = default; Tracking_loop_filter(float update_interval, float noise_bandwidth, int loop_order = 2, bool include_last_integrator = false); - Tracking_loop_filter& operator=(Tracking_loop_filter&& other); //!< Move assignment operator + Tracking_loop_filter(Tracking_loop_filter&&) = default; //!< Move operator + Tracking_loop_filter& operator=(Tracking_loop_filter&& /*other*/) = default; //!< Move assignment operator float get_noise_bandwidth(void) const; float get_update_interval(void) const; diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 935c292de..6cf746636 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -56,7 +56,7 @@ public: { google::protobuf::ShutdownProtobufLibrary(); } - /* + inline Serdes_Gnss_Synchro(Serdes_Gnss_Synchro&& other) //!< Copy constructor { this->observables = other.observables; @@ -64,12 +64,7 @@ public: inline Serdes_Gnss_Synchro& operator=(const Serdes_Gnss_Synchro& rhs) //!< Copy assignment operator { - // Only do assignment if RHS is a different object from this. - if (this != &rhs) - { - // Deallocate, allocate new space, copy values... - this->observables = rhs.observables; - } + this->observables = rhs.observables; return *this; } @@ -85,7 +80,7 @@ public: this->observables = std::move(other.observables); } return *this; - }*/ + } inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { From 8de5ea8737c9a2fb9ba3a3bdab70530bf353c946 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 21:50:03 +0200 Subject: [PATCH 60/64] Fix comparison --- src/algorithms/input_filter/gnuradio_blocks/beamformer.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc index 165890e66..726ae6bd2 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc @@ -67,7 +67,7 @@ int beamformer::work(int noutput_items, gr_vector_const_void_star &input_items, for (int n = 0; n < noutput_items; n++) { sum = gr_complex(0, 0); - for (int i = 0; i < weight_vector.size(); i++) + for (unsigned int i = 0; i < weight_vector.size(); i++) { sum = sum + (reinterpret_cast(input_items[i]))[n] * weight_vector[i]; } From 24007b59e52ff14d1c2c3bd5183bf25608a4ccee Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 22:36:29 +0200 Subject: [PATCH 61/64] Remove unused includes, fix docs --- src/core/system_parameters/Beidou_B1I.h | 15 +++++++-------- src/core/system_parameters/Beidou_B3I.h | 5 ++--- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index bdc5ff40f..a341d9a2c 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -34,8 +34,7 @@ #include "MATH_CONSTANTS.h" #include -#include // std::pair -#include +#include // Physical constants const double BEIDOU_C_M_S = 299792458.0; //!< The speed of light, [m/s] @@ -48,12 +47,12 @@ const double BEIDOU_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2) // carrier and code frequencies -const double BEIDOU_B1I_FREQ_HZ = 1.561098e9; //!< b1I [Hz] -const double BEIDOU_B1I_CODE_RATE_HZ = 2.046e6; //!< beidou b1I code rate [chips/s] -const double BEIDOU_B1I_CODE_LENGTH_CHIPS = 2046.0; //!< beidou b1I code length [chips] -const double BEIDOU_B1I_CODE_PERIOD = 0.001; //!< beidou b1I code period [seconds] -const uint32_t BEIDOU_B1I_CODE_PERIOD_MS = 1; //!< beidou b1I L1 C/A code period [ms] -const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< beidou b1I chip period [seconds] +const double BEIDOU_B1I_FREQ_HZ = 1.561098e9; //!< B1I [Hz] +const double BEIDOU_B1I_CODE_RATE_HZ = 2.046e6; //!< Beidou B1I code rate [chips/s] +const double BEIDOU_B1I_CODE_LENGTH_CHIPS = 2046.0; //!< Beidou B1I code length [chips] +const double BEIDOU_B1I_CODE_PERIOD = 0.001; //!< Beidou B1I code period [seconds] +const uint32_t BEIDOU_B1I_CODE_PERIOD_MS = 1; //!< Beidou B1I code period [ms] +const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< Beidou B1I chip period [seconds] const int32_t BEIDOU_B1I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B1I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B1I_SECONDARY_CODE_STR = "00000100110101001110"; diff --git a/src/core/system_parameters/Beidou_B3I.h b/src/core/system_parameters/Beidou_B3I.h index 3eceab185..bf77457f5 100644 --- a/src/core/system_parameters/Beidou_B3I.h +++ b/src/core/system_parameters/Beidou_B3I.h @@ -33,15 +33,14 @@ #include "MATH_CONSTANTS.h" #include -#include // std::pair -#include +#include // carrier and code frequencies const double BEIDOU_B3I_FREQ_HZ = 1.268520e9; //!< BeiDou B3I [Hz] const double BEIDOU_B3I_CODE_RATE_HZ = 10.23e6; //!< BeiDou B3I code rate [chips/s] const double BEIDOU_B3I_CODE_LENGTH_CHIPS = 10230.0; //!< BeiDou B3I code length [chips] const double BEIDOU_B3I_CODE_PERIOD = 0.001; //!< BeiDou B3I code period [seconds] -const uint32_t BEIDOU_B3I_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code period [ms] +const uint32_t BEIDOU_B3I_CODE_PERIOD_MS = 1; //!< BeiDou B3I code period [ms] const int32_t BEIDOU_B3I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B3I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B3I_SECONDARY_CODE_STR = "00000100110101001110"; From faeb99bf6f057811c094843b09ff7c59967be926 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Jul 2019 20:00:29 +0200 Subject: [PATCH 62/64] Fix GLONASS L1 nav message decoding (fixes #289) --- .../glonass_l1_ca_telemetry_decoder_gs.cc | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 3f6fdabcb..e31ae740a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -100,13 +100,11 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( } } - d_symbol_history.set_capacity(GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS); + d_symbol_history.set_capacity(GLONASS_GNAV_STRING_SYMBOLS); d_sample_counter = 0ULL; d_stat = 0; d_preamble_index = 0ULL; - d_flag_frame_sync = false; - d_flag_parity = false; d_TOW_at_current_symbol = 0; Flag_valid_word = false; @@ -289,13 +287,12 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu d_flag_preamble = false; - if (static_cast(d_symbol_history.size()) == d_symbols_per_preamble) + if (static_cast(d_symbol_history.size()) >= d_symbols_per_preamble) { // ******* preamble correlation ******** - int i = 0; - for (const auto &iter : d_symbol_history) + for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - if (iter.Prompt_I < 0.0) // symbols clipping + if (d_symbol_history[i].Prompt_I < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; } @@ -303,7 +300,6 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu { corr_value += d_preambles_symbols[i]; } - i++; } } From d6dd4cad7793888c887f67b89be055d0d3c28053 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Jul 2019 20:09:34 +0200 Subject: [PATCH 63/64] Fix GLONASS L2 nav message decoding --- .../glonass_l2_ca_telemetry_decoder_gs.cc | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index e26592279..053596048 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -100,7 +100,7 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( } } - d_symbol_history.set_capacity(GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS); + d_symbol_history.set_capacity(GLONASS_GNAV_STRING_SYMBOLS); d_sample_counter = 0ULL; d_stat = 0; d_preamble_index = 0ULL; @@ -289,13 +289,12 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu d_flag_preamble = false; - if (d_symbol_history.size() == GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) + if (static_cast(d_symbol_history.size()) >= d_symbols_per_preamble) { // ******* preamble correlation ******** - int i = 0; - for (const auto &iter : d_symbol_history) + for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - if (iter.Prompt_I < 0.0) // symbols clipping + if (d_symbol_history.[i].Prompt_I < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; } @@ -303,7 +302,6 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu { corr_value += d_preambles_symbols[i]; } - i++; } } From 38559628e890efe984551fb7a74c57710b7c0c8c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Jul 2019 20:21:58 +0200 Subject: [PATCH 64/64] Fix bug --- .../gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index 053596048..3fce9c02e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -294,7 +294,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu // ******* preamble correlation ******** for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - if (d_symbol_history.[i].Prompt_I < 0.0) // symbols clipping + if (d_symbol_history[i].Prompt_I < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; }