diff --git a/.gitignore b/.gitignore index ac9786e58866aa41fb04ebfbfe2890352bf58c21..d2a777ed4f2abca8c254a4ab2eecd47727162068 100644 --- a/.gitignore +++ b/.gitignore @@ -1,265 +1,7 @@ -lib/libgnss_utils.dylib -deps/RTKLIB -build/test/Makefile -build/test/gtest/tmp/googletest-gitupdate.cmake -build/test/gtest/tmp/googletest-gitclone.cmake -build/test/gtest/tmp/googletest-cfgcmd.txt.in -build/test/gtest/tmp/googletest-cfgcmd.txt -build/test/gtest/src/googletest/ -build/test/gtest/src/googletest-stamp/googletest-patch -build/test/gtest/src/googletest-stamp/googletest-mkdir -build/test/gtest/src/googletest-stamp/googletest-install -build/test/gtest/src/googletest-stamp/googletest-gitinfo.txt -build/test/gtest/src/googletest-stamp/googletest-gitclone-lastrun.txt -build/test/gtest/src/googletest-stamp/googletest-download -build/test/gtest/src/googletest-stamp/googletest-done -build/test/gtest/src/googletest-stamp/googletest-configure -build/test/gtest/src/googletest-stamp/googletest-build -build/test/gtest/src/googletest-build/Makefile -build/test/gtest/src/googletest-build/googlemock/Makefile -build/test/gtest/src/googletest-build/googlemock/libgmock_main.a -build/test/gtest/src/googletest-build/googlemock/libgmock.a -build/test/gtest/src/googletest-build/googlemock/gtest/Makefile -build/test/gtest/src/googletest-build/googlemock/gtest/libgtest_main.a -build/test/gtest/src/googletest-build/googlemock/gtest/libgtest.a -build/test/gtest/src/googletest-build/googlemock/gtest/generated/GTestConfigVersion.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/generated/GTestConfig.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/generated/gtest_main.pc -build/test/gtest/src/googletest-build/googlemock/gtest/generated/gtest.pc -build/test/gtest/src/googletest-build/googlemock/gtest/generated/gmock_main.pc -build/test/gtest/src/googletest-build/googlemock/gtest/generated/gmock.pc -build/test/gtest/src/googletest-build/googlemock/gtest/CTestTestfile.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/progress.marks -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/progress.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/link.txt -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/flags.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/depend.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/depend.internal -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/CXX.includecache -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/cmake_clean_target.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest_main.dir/build.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/progress.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/link.txt -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/flags.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/DependInfo.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/depend.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/depend.internal -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/CXX.includecache -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/cmake_clean_target.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/gtest.dir/build.make -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/Export/lib/cmake/GTest/GTestTargets.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/Export/lib/cmake/GTest/GTestTargets-noconfig.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/CMakeFiles/CMakeDirectoryInformation.cmake -build/test/gtest/src/googletest-build/googlemock/gtest/cmake_install.cmake -build/test/gtest/src/googletest-build/googlemock/CTestTestfile.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/progress.marks -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/src/gmock_main.cc.o -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/progress.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/link.txt -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/flags.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/DependInfo.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/depend.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/depend.internal -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/CXX.includecache -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/cmake_clean_target.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/cmake_clean.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock_main.dir/build.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/src/gmock-all.cc.o -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/progress.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/link.txt -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/flags.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/DependInfo.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/depend.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/depend.internal -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/CXX.includecache -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/cmake_clean_target.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/cmake_clean.cmake -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/gmock.dir/build.make -build/test/gtest/src/googletest-build/googlemock/CMakeFiles/CMakeDirectoryInformation.cmake -build/test/gtest/src/googletest-build/googlemock/cmake_install.cmake -build/test/gtest/src/googletest-build/CTestTestfile.cmake -build/test/gtest/src/googletest-build/CMakeFiles/TargetDirectories.txt -build/test/gtest/src/googletest-build/CMakeFiles/progress.marks -build/test/gtest/src/googletest-build/CMakeFiles/Makefile2 -build/test/gtest/src/googletest-build/CMakeFiles/Makefile.cmake -build/test/gtest/src/googletest-build/CMakeFiles/feature_tests.cxx -build/test/gtest/src/googletest-build/CMakeFiles/feature_tests.c -build/test/gtest/src/googletest-build/CMakeFiles/feature_tests.bin -build/test/gtest/src/googletest-build/CMakeFiles/CMakeOutput.log -build/test/gtest/src/googletest-build/CMakeFiles/CMakeDirectoryInformation.cmake -build/test/gtest/src/googletest-build/CMakeFiles/cmake.check_cache -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CompilerIdCXX/CMakeCXXCompilerId.cpp -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CompilerIdCXX/a.out -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CompilerIdC/CMakeCCompilerId.c -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CompilerIdC/a.out -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CMakeSystem.cmake -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CMakeDetermineCompilerABI_CXX.bin -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CMakeDetermineCompilerABI_C.bin -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CMakeCXXCompiler.cmake -build/test/gtest/src/googletest-build/CMakeFiles/3.14.4/CMakeCCompiler.cmake -build/test/gtest/src/googletest-build/CMakeCache.txt -build/test/gtest/src/googletest-build/cmake_install.cmake -build/test/gtest/Makefile -build/test/gtest/CTestTestfile.cmake -build/test/gtest/CMakeFiles/progress.marks -build/test/gtest/CMakeFiles/googletest.dir/progress.make -build/test/gtest/CMakeFiles/googletest.dir/Labels.txt -build/test/gtest/CMakeFiles/googletest.dir/Labels.json -build/test/gtest/CMakeFiles/googletest.dir/DependInfo.cmake -build/test/gtest/CMakeFiles/googletest.dir/depend.make -build/test/gtest/CMakeFiles/googletest.dir/depend.internal -build/test/gtest/CMakeFiles/googletest.dir/cmake_clean.cmake -build/test/gtest/CMakeFiles/googletest.dir/build.make -build/test/gtest/CMakeFiles/googletest-complete -build/test/gtest/CMakeFiles/CMakeDirectoryInformation.cmake -build/test/gtest/cmake_install.cmake -build/test/CTestTestfile.cmake -build/test/CMakeFiles/progress.marks -build/test/CMakeFiles/gtest_transformations.dir/progress.make -build/test/CMakeFiles/gtest_transformations.dir/link.txt -build/test/CMakeFiles/gtest_transformations.dir/gtest_transformations.o -build/test/CMakeFiles/gtest_transformations.dir/flags.make -build/test/CMakeFiles/gtest_transformations.dir/DependInfo.cmake -build/test/CMakeFiles/gtest_transformations.dir/depend.make -build/test/CMakeFiles/gtest_transformations.dir/depend.internal -build/test/CMakeFiles/gtest_transformations.dir/CXX.includecache -build/test/CMakeFiles/gtest_transformations.dir/cmake_clean.cmake -build/test/CMakeFiles/gtest_transformations.dir/build.make -build/test/CMakeFiles/gtest_observations.dir/progress.make -build/test/CMakeFiles/gtest_observations.dir/link.txt -build/test/CMakeFiles/gtest_observations.dir/gtest_observations.o -build/test/CMakeFiles/gtest_observations.dir/flags.make -build/test/CMakeFiles/gtest_observations.dir/DependInfo.cmake -build/test/CMakeFiles/gtest_observations.dir/depend.make -build/test/CMakeFiles/gtest_observations.dir/depend.internal -build/test/CMakeFiles/gtest_observations.dir/CXX.includecache -build/test/CMakeFiles/gtest_observations.dir/cmake_clean.cmake -build/test/CMakeFiles/gtest_observations.dir/build.make -build/test/CMakeFiles/gtest_navigation.dir/progress.make -build/test/CMakeFiles/gtest_navigation.dir/link.txt -build/test/CMakeFiles/gtest_navigation.dir/gtest_navigation.o -build/test/CMakeFiles/gtest_navigation.dir/flags.make -build/test/CMakeFiles/gtest_navigation.dir/DependInfo.cmake -build/test/CMakeFiles/gtest_navigation.dir/depend.make -build/test/CMakeFiles/gtest_navigation.dir/depend.internal -build/test/CMakeFiles/gtest_navigation.dir/CXX.includecache -build/test/CMakeFiles/gtest_navigation.dir/cmake_clean.cmake -build/test/CMakeFiles/gtest_navigation.dir/build.make -build/test/CMakeFiles/CMakeDirectoryInformation.cmake -build/test/cmake_install.cmake -build/src/examples/Makefile -build/src/examples/CTestTestfile.cmake -build/src/examples/CMakeFiles/progress.marks -build/src/examples/CMakeFiles/gnss_utils_test.dir/progress.make -build/src/examples/CMakeFiles/gnss_utils_test.dir/link.txt -build/src/examples/CMakeFiles/gnss_utils_test.dir/gnss_utils_test.o -build/src/examples/CMakeFiles/gnss_utils_test.dir/flags.make -build/src/examples/CMakeFiles/gnss_utils_test.dir/DependInfo.cmake -build/src/examples/CMakeFiles/gnss_utils_test.dir/depend.make -build/src/examples/CMakeFiles/gnss_utils_test.dir/depend.internal -build/src/examples/CMakeFiles/gnss_utils_test.dir/CXX.includecache -build/src/examples/CMakeFiles/gnss_utils_test.dir/cmake_clean.cmake -build/src/examples/CMakeFiles/gnss_utils_test.dir/build.make -build/src/examples/CMakeFiles/CMakeDirectoryInformation.cmake -build/src/examples/cmake_install.cmake -build/Makefile -build/install_manifest.txt -build/CTestTestfile.cmake -build/conf/gnss_utils/internal/config.h -build/CMakeFiles/uninstall.dir/progress.make -build/CMakeFiles/uninstall.dir/DependInfo.cmake -build/CMakeFiles/uninstall.dir/cmake_clean.cmake -build/CMakeFiles/uninstall.dir/build.make -build/CMakeFiles/TargetDirectories.txt -build/CMakeFiles/progress.marks -build/CMakeFiles/Makefile2 -build/CMakeFiles/Makefile.cmake -build/CMakeFiles/gnss_utils.dir/src/utils/utils.o -build/CMakeFiles/gnss_utils.dir/src/utils/transformations.o -build/CMakeFiles/gnss_utils.dir/src/utils/satellite.o -build/CMakeFiles/gnss_utils.dir/src/utils/rcv_position.o -build/CMakeFiles/gnss_utils.dir/src/tdcp.o -build/CMakeFiles/gnss_utils.dir/src/snapshot.o -build/CMakeFiles/gnss_utils.dir/src/receivers/ublox_raw.o -build/CMakeFiles/gnss_utils.dir/src/receivers/novatel_raw.o -build/CMakeFiles/gnss_utils.dir/src/receiver_raw_base.o -build/CMakeFiles/gnss_utils.dir/src/observations.o -build/CMakeFiles/gnss_utils.dir/src/navigation.o -build/CMakeFiles/gnss_utils.dir/progress.make -build/CMakeFiles/gnss_utils.dir/link.txt -build/CMakeFiles/gnss_utils.dir/flags.make -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/sbas.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rtkcmn.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rtcm3e.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rtcm3.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rtcm2.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rtcm.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rinex.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcvraw.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/ublox.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/tersus.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/swiftnav.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/skytraq.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/septentrio.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/rt17.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/nvs.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/novatel.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/javad.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/gw10.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/crescent.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/comnav.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/cmr.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/rcv/binex.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/qzslex.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/preceph.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/pntpos.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/ionex.o -build/CMakeFiles/gnss_utils.dir/deps/RTKLIB/src/ephemeris.o -build/CMakeFiles/gnss_utils.dir/DependInfo.cmake -build/CMakeFiles/gnss_utils.dir/depend.make -build/CMakeFiles/gnss_utils.dir/depend.internal -build/CMakeFiles/gnss_utils.dir/CXX.includecache -build/CMakeFiles/gnss_utils.dir/cmake_clean.cmake -build/CMakeFiles/gnss_utils.dir/C.includecache -build/CMakeFiles/gnss_utils.dir/build.make -build/CMakeFiles/feature_tests.cxx -build/CMakeFiles/feature_tests.c -build/CMakeFiles/feature_tests.bin -build/CMakeFiles/doc.dir/progress.make -build/CMakeFiles/doc.dir/DependInfo.cmake -build/CMakeFiles/doc.dir/cmake_clean.cmake -build/CMakeFiles/doc.dir/build.make -build/CMakeFiles/distclean.dir/progress.make -build/CMakeFiles/distclean.dir/DependInfo.cmake -build/CMakeFiles/distclean.dir/cmake_clean.cmake -build/CMakeFiles/distclean.dir/build.make -build/CMakeFiles/CMakeRuleHashes.txt -build/CMakeFiles/CMakeOutput.log -build/CMakeFiles/CMakeDirectoryInformation.cmake -build/CMakeFiles/cmake.check_cache -build/CMakeFiles/3.14.4/CompilerIdCXX/CMakeCXXCompilerId.cpp -build/CMakeFiles/3.14.4/CompilerIdCXX/a.out -build/CMakeFiles/3.14.4/CompilerIdC/CMakeCCompilerId.c -build/CMakeFiles/3.14.4/CompilerIdC/a.out -build/CMakeFiles/3.14.4/CMakeSystem.cmake -build/CMakeFiles/3.14.4/CMakeDetermineCompilerABI_CXX.bin -build/CMakeFiles/3.14.4/CMakeDetermineCompilerABI_C.bin -build/CMakeFiles/3.14.4/CMakeCXXCompiler.cmake -build/CMakeFiles/3.14.4/CMakeCCompiler.cmake -build/CMakeDoxygenDefaults.cmake -build/CMakeDoxyfile.in -build/CMakeCache.txt -build/cmake_install.cmake -bin/gtest_transformations -bin/gtest_observations -bin/gtest_navigation -bin/gnss_utils_test +lib/* +build/* +bin/* .settings/language.settings.xml .project .cproject diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ec32eefac1059a24af1a451a0352f80f51d4e83..37953bb9fe7012aa1d457d3389bdac417077b304 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,17 +77,18 @@ SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src) # driver source files SET(SOURCES - src/utils/utils.cpp - src/utils/transformations.cpp - src/utils/rcv_position.cpp - src/utils/satellite.cpp - src/observations.cpp src/navigation.cpp - src/snapshot.cpp - src/tdcp.cpp + src/observations.cpp + src/range.cpp src/receiver_raw_base.cpp src/receivers/ublox_raw.cpp - src/receivers/novatel_raw.cpp) + src/receivers/novatel_raw.cpp + src/snapshot.cpp + src/tdcp.cpp + src/utils/utils.cpp + src/utils/transformations.cpp + src/utils/rcv_position.cpp + src/utils/satellite.cpp) SET(RTKLIB_SRC ${RTKLIB_SRC_DIR}/pntpos.c diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index a7714735bd61f66c23240896b476390848b3c004..0102a6c72bdafe08efeb4e3ae61e9d112221948c 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -27,20 +27,6 @@ namespace GnssUtils { // Structs -struct PseudoRange -{ - int sat = 0; - double p = -1; - double prange_var = 1; - double prange = -1; - double iono_corr = 0; - double tropo_corr = 0; - double sat_clock_corr = 0; - double L = -1; - double carrier_range = -1; - double carrier_range_var = 1; -}; - struct ComputePosOutput { time_t time; @@ -114,6 +100,7 @@ struct TdcpOptions bool enabled; // TDCP enabled bool corr_iono; // apply correction also in TDCP bool corr_tropo; // apply correction also in TDCP + bool corr_clock; bool loss_function; // apply loss function in TDCP factors double sigma_atm; double sigma_carrier; @@ -179,10 +166,12 @@ class Observations; class Navigation; class Snapshot; struct Satellite; +class Range; // Typedefs typedef std::map<int,Satellite> Satellites; -typedef std::map<int,PseudoRange> PseudoRanges; +typedef std::map<int,Range> Ranges; +typedef std::map<int,Eigen::Vector2d> Azels; // pointer typedefs typedef std::shared_ptr<Observations> ObservationsPtr; diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h index 91deec41d86cc40b6e2ef0b6aa5d77083deb873d..468c418d2e618647e423a76d42c428c361a4908a 100644 --- a/include/gnss_utils/observations.h +++ b/include/gnss_utils/observations.h @@ -47,7 +47,7 @@ public: const snrmask_t& snrmask, const double& elmin, const bool& multi_freq); - std::set<int> filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels, + std::set<int> filterByElevationSnr(const Azels& azels, const snrmask_t& snrmask, const double& elmin, const bool& multi_freq); @@ -66,21 +66,21 @@ public: const snrmask_t& snrmask, const double& elmin, const bool& multi_freq); - std::set<int> filter(const Satellites& sats, - const std::set<int>& discarded_sats, - const std::map<int,Eigen::Vector2d>& azels, - const bool& check_code, - const bool& check_carrier_phase, - const Options& opt); - std::set<int> filter(const Satellites& sats, - const std::set<int>& discarded_sats, - const std::map<int,Eigen::Vector2d>& azels, - const bool& check_code, - const bool& check_carrier_phase, - const int& navsys, - const snrmask_t& snrmask, - const double& elmin, - const bool& multi_freq); + std::set<int> filter(const Satellites& sats, + const std::set<int>& discarded_sats, + const Azels& azels, + const bool& check_code, + const bool& check_carrier_phase, + const Options& opt); + std::set<int> filter(const Satellites& sats, + const std::set<int>& discarded_sats, + const Azels& azels, + const bool& check_code, + const bool& check_carrier_phase, + const int& navsys, + const snrmask_t& snrmask, + const double& elmin, + const bool& multi_freq); // Others static std::set<int> findCommonObservations(const Observations& obs_1, const Observations& obs_2); diff --git a/include/gnss_utils/range.h b/include/gnss_utils/range.h new file mode 100644 index 0000000000000000000000000000000000000000..caf9e9db23c97692ba3e4bb1a438e8162b5c24ea --- /dev/null +++ b/include/gnss_utils/range.h @@ -0,0 +1,46 @@ +/* + * range.h + * + * Created on: May 28, 2020 + * Author: joanvallve + */ + +#ifndef INCLUDE_GNSS_UTILS_RANGE_H_ +#define INCLUDE_GNSS_UTILS_RANGE_H_ + +#include "gnss_utils/gnss_utils.h" +#include "gnss_utils/observations.h" +#include "gnss_utils/navigation.h" + +namespace GnssUtils +{ + +class Range +{ + public: + int sat = 0; + double P = -1; + double P_var = 1; + double P_corrected = -1; + double iono_corr = 0; + double tropo_corr = 0; + double sat_clock_corr = 0; + double L = -1; + double L_corrected = -1; + double L_var = 1; + + public: + Range(); + virtual ~Range(); + + static Ranges computeRanges(ObservationsPtr obs, + NavigationPtr nav, + const Satellites& sats, + const Azels& azel, + const Eigen::Vector3d& latlonalt, + const Options& opt); +}; + +} /* namespace GnssUtils */ + +#endif /* INCLUDE_GNSS_UTILS_RANGE_H_ */ diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h index 04e2d6da55156844c9636cbd0bdfdf05a0f95996..732d1913292feb8c3a7c48ee6091e172beaac10f 100644 --- a/include/gnss_utils/snapshot.h +++ b/include/gnss_utils/snapshot.h @@ -38,24 +38,24 @@ public: void computeSatellites(const int& eph_opt); // see rtklib.h L396); bool satellitesComputed() const; - std::set<int> filterObservations(const std::set<int> &discarded_sats, - const Eigen::Vector3d &x_r, - const bool &check_code, - const bool &check_carrier_phase, - const Options &opt); - std::set<int> filterObservations(const std::set<int> &discarded_sats, - const std::map<int,Eigen::Vector2d>& azels, - const bool &check_code, - const bool &check_carrier_phase, - const Options &opt); + std::set<int> filterObservations(const std::set<int>& discarded_sats, + const Eigen::Vector3d& x_r, + const bool& check_code, + const bool& check_carrier_phase, + const Options& opt); + std::set<int> filterObservations(const std::set<int>& discarded_sats, + const Azels& azels, + const bool& check_code, + const bool& check_carrier_phase, + const Options& opt); // Pseudo-ranges - PseudoRanges& getPseudoRanges(); - const PseudoRanges& getPseudoRanges() const; - bool pseudoRangesComputed() const; - void computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, - const Eigen::Vector3d& latlonalt, - const Options& opt); + Ranges& getRanges(); + const Ranges& getRanges() const; + bool rangesComputed() const; + void computeRanges(const Azels& azel, + const Eigen::Vector3d& latlonalt, + const Options& opt); void print(); @@ -64,7 +64,7 @@ private: Satellites sats_; ObservationsPtr obs_; NavigationPtr nav_; - PseudoRanges pranges_; + Ranges ranges_; // Private methods }; @@ -76,6 +76,7 @@ private: #include "gnss_utils/observations.h" #include "gnss_utils/navigation.h" #include "gnss_utils/utils/utils.h" +#include "gnss_utils/range.h" namespace GnssUtils { @@ -115,37 +116,37 @@ inline bool Snapshot::satellitesComputed() const return !sats_.empty(); } -inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded_sats, - const Eigen::Vector3d &x_r, - const bool &check_code, - const bool &check_carrier_phase, - const Options &opt) +inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats, + const Eigen::Vector3d& x_r, + const bool& check_code, + const bool& check_carrier_phase, + const Options& opt) { return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt); } -inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded_sats, - const std::map<int,Eigen::Vector2d>& azels, - const bool &check_code, - const bool &check_carrier_phase, - const Options &opt) +inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats, + const Azels& azels, + const bool& check_code, + const bool& check_carrier_phase, + const Options& opt) { return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt); } -inline const PseudoRanges& Snapshot::getPseudoRanges() const +inline const Ranges& Snapshot::getRanges() const { - return pranges_; + return ranges_; } -inline PseudoRanges& Snapshot::getPseudoRanges() +inline Ranges& Snapshot::getRanges() { - return pranges_; + return ranges_; } -inline bool Snapshot::pseudoRangesComputed() const +inline bool Snapshot::rangesComputed() const { - return !pranges_.empty(); + return !ranges_.empty(); } } // namespace GnssUtils diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h index c73b035d8d8bd57f2190662fbcb3549b5dcb5d40..b5a8d134e8aef41cc06accb428e0319ded3ee340 100644 --- a/include/gnss_utils/utils/utils.h +++ b/include/gnss_utils/utils/utils.h @@ -24,7 +24,7 @@ void print(std::string& _msg); template <typename T, size_t size> void printArray(std::string name, T (&array)[size]) { - std::cout << name << ": ["; + std::cout << name << "["; for (int ii = 0; ii < size; ++ii) { if (std::is_same<T, char>::value or std::is_same<T, unsigned char>::value) diff --git a/src/navigation.cpp b/src/navigation.cpp index af421b711f719a741451bbdf9093a20b252b2614..c0eb72c4681d6cb650e960e8fba5ac50cd4bc5db 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -283,25 +283,16 @@ void Navigation::freeNavigationArrays() void Navigation::print() { - std::cout << "n: " << nav_.n << "\n"; - std::cout << "ng: " << nav_.ng << "\n"; - std::cout << "ns: " << nav_.ns << "\n"; - std::cout << "ne: " << nav_.ne << "\n"; - std::cout << "nc: " << nav_.nc << "\n"; - std::cout << "na: " << nav_.na << "\n"; - std::cout << "nt: " << nav_.nt << "\n"; - std::cout << "nf: " << nav_.nf << "\n"; - - std::cout << "nmax: " << nav_.nmax << "\n"; - std::cout << "ngmax: " << nav_.ngmax << "\n"; - std::cout << "nsmax: " << nav_.nsmax << "\n"; - std::cout << "nemax: " << nav_.nemax << "\n"; - std::cout << "ncmax: " << nav_.ncmax << "\n"; - std::cout << "namax: " << nav_.namax << "\n"; - std::cout << "ntmax: " << nav_.ntmax << "\n"; - std::cout << "nfmax: " << nav_.nfmax << "\n"; - - // TODO + std::cout << "n: " << nav_.n << "\t(nmax: " << nav_.nmax << ")\n"; + std::cout << "ng: " << nav_.ng << "\t(nfmax: " << nav_.nfmax << ")\n"; + std::cout << "ns: " << nav_.ns << "\t(nsmax: " << nav_.nsmax << ")\n"; + std::cout << "ne: " << nav_.ne << "\t(nemax: " << nav_.nemax << ")\n"; + std::cout << "nc: " << nav_.nc << "\t(ncmax: " << nav_.ncmax << ")\n"; + std::cout << "na: " << nav_.na << "\t(namax: " << nav_.namax << ")\n"; + std::cout << "nt: " << nav_.nt << "\t(ntmax: " << nav_.ntmax << ")\n"; + std::cout << "nf: " << nav_.nf << "\t(nfmax: " << nav_.nfmax << ")\n"; + + // TODO // eph_t *eph; /* GPS/QZS/GAL ephemeris */ // geph_t *geph; /* GLONASS ephemeris */ // seph_t *seph; /* SBAS ephemeris */ @@ -312,68 +303,102 @@ void Navigation::print() // fcbd_t *fcb; /* satellite fcb data */ // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ - printArray<double, ARRAY_SIZE(nav_.utc_gps)>("utc_gps: ", nav_.utc_gps); - printArray<double, ARRAY_SIZE(nav_.utc_glo)>("utc_glo: ", nav_.utc_glo); - printArray<double, ARRAY_SIZE(nav_.utc_qzs)>("utc_qzs: ", nav_.utc_qzs); - printArray<double, ARRAY_SIZE(nav_.utc_cmp)>("utc_cmp: ", nav_.utc_cmp); - printArray<double, ARRAY_SIZE(nav_.utc_irn)>("utc_irn: ", nav_.utc_irn); - printArray<double, ARRAY_SIZE(nav_.utc_sbs)>("utc_sbs: ", nav_.utc_sbs); - printArray<double, ARRAY_SIZE(nav_.ion_gps)>("ion_gps: ", nav_.ion_gps); - printArray<double, ARRAY_SIZE(nav_.ion_gal)>("ion_gal: ", nav_.ion_gal); - printArray<double, ARRAY_SIZE(nav_.ion_qzs)>("ion_qzs: ", nav_.ion_qzs); - printArray<double, ARRAY_SIZE(nav_.ion_cmp)>("ion_cmp: ", nav_.ion_cmp); - printArray<double, ARRAY_SIZE(nav_.ion_irn)>("ion_irn: ", nav_.ion_irn); - - std::cout << "leaps: " << nav_.leaps << "\n"; - - std::cout << "sbssat: \n"; - std::cout << "\tiodp: " << nav_.sbssat.iodp << "\n"; - std::cout << "\tnsat: " << nav_.sbssat.nsat << "\n"; - std::cout << "\ttlat: " << nav_.sbssat.tlat << "\n"; - for (int i = 0; i < nav_.sbssat.nsat; i++) + printArray<double, ARRAY_SIZE(nav_.utc_gps)>("utc_gps: ", nav_.utc_gps); + printArray<double, ARRAY_SIZE(nav_.utc_glo)>("utc_glo: ", nav_.utc_glo); + printArray<double, ARRAY_SIZE(nav_.utc_gal)>("utc_gal: ", nav_.utc_gal); + printArray<double, ARRAY_SIZE(nav_.utc_qzs)>("utc_qzs: ", nav_.utc_qzs); + printArray<double, ARRAY_SIZE(nav_.utc_cmp)>("utc_cmp: ", nav_.utc_cmp); + printArray<double, ARRAY_SIZE(nav_.utc_irn)>("utc_irn: ", nav_.utc_irn); + printArray<double, ARRAY_SIZE(nav_.utc_sbs)>("utc_sbs: ", nav_.utc_sbs); + printArray<double, ARRAY_SIZE(nav_.ion_gps)>("ion_gps: ", nav_.ion_gps); + printArray<double, ARRAY_SIZE(nav_.ion_gal)>("ion_gal: ", nav_.ion_gal); + printArray<double, ARRAY_SIZE(nav_.ion_qzs)>("ion_qzs: ", nav_.ion_qzs); + printArray<double, ARRAY_SIZE(nav_.ion_cmp)>("ion_cmp: ", nav_.ion_cmp); + printArray<double, ARRAY_SIZE(nav_.ion_irn)>("ion_irn: ", nav_.ion_irn); + + std::cout << "leaps: " << nav_.leaps << "\n"; + + std::cout << "lam: \n"; + for (int i = 0; i < ARRAY2D_NROWS(nav_.lam); i++) + printArray<double, ARRAY2D_NCOLS(nav_.lam)>("\t", nav_.lam[i]); + + std::cout << "cbias: \n"; + for (int i = 0; i < ARRAY2D_NROWS(nav_.cbias); i++) + printArray<double, ARRAY2D_NCOLS(nav_.cbias)>("\t", nav_.cbias[i]); + + printArray<double, ARRAY_SIZE(nav_.wlbias)> ("wlbias: ", nav_.wlbias); + printArray<double, ARRAY_SIZE(nav_.glo_cpbias)>("glo_cpbias: ", nav_.glo_cpbias); + printArray<char, ARRAY_SIZE(nav_.glo_fcn)> ("glo_fcn: ", nav_.glo_fcn); + + std::cout << "pcvs: \n"; + for (int i = 0; i < ARRAY_SIZE(nav_.pcvs); i++) { - std::cout << "\tsat: " << i << "\n"; - std::cout << "\t\tsat " << nav_.sbssat.sat[i].sat << "\n"; - - std::cout << "\t\tfcorr.t0 " << nav_.sbssat.sat[i].fcorr.t0.time << " + "<< nav_.sbssat.sat[i].fcorr.t0.sec << "\n"; - std::cout << "\t\tfcorr.prc " << nav_.sbssat.sat[i].fcorr.prc << "\n"; - std::cout << "\t\tfcorr.rrc " << nav_.sbssat.sat[i].fcorr.rrc << "\n"; - std::cout << "\t\tfcorr.dt " << nav_.sbssat.sat[i].fcorr.dt << "\n"; - std::cout << "\t\tfcorr.iodf " << nav_.sbssat.sat[i].fcorr.iodf << "\n"; - std::cout << "\t\tfcorr.udre " << nav_.sbssat.sat[i].fcorr.udre << "\n"; - std::cout << "\t\tfcorr.ai " << nav_.sbssat.sat[i].fcorr.ai << "\n"; - - std::cout << "\t\tlcorr.daf0 " << nav_.sbssat.sat[i].lcorr.daf0 << "\n"; - std::cout << "\t\tlcorr.daf1 " << nav_.sbssat.sat[i].lcorr.daf1 << "\n"; - std::cout << "\t\tlcorr.iode " << nav_.sbssat.sat[i].lcorr.iode << "\n"; - std::cout << "\t\tlcorr.t0 " << nav_.sbssat.sat[i].lcorr.t0.time << " + "<< nav_.sbssat.sat[i].lcorr.t0.sec << "\n"; - printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dpos)>("\t\tlcorr.dpos: ", nav_.sbssat.sat[i].lcorr.dpos); - printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dvel)>("\t\tlcorr.dvel: ", nav_.sbssat.sat[i].lcorr.dvel); + std::cout << "\tsat: " << nav_.pcvs[i].sat << "\n"; + printArray<char, ARRAY_SIZE(nav_.pcvs[i].type)>("\t\ttype: ", nav_.pcvs[i].type); + printArray<char, ARRAY_SIZE(nav_.pcvs[i].code)>("\t\tcode: ", nav_.pcvs[i].code); + + std::cout << "\t\tte: " << nav_.pcvs[i].te.time << " + "<< nav_.pcvs[i].te.sec << "\n"; + std::cout << "\t\tts: " << nav_.pcvs[i].ts.time << " + "<< nav_.pcvs[i].ts.sec << "\n"; + std::cout << "\t\toff: \n"; + for (int j = 0; j < ARRAY2D_NROWS(nav_.pcvs[i].off); j++) + printArray<double, ARRAY2D_NCOLS(nav_.pcvs[i].off)>("\t\t\t", nav_.pcvs[i].off[j]); + std::cout << "\t\tvar: \n"; + for (int j = 0; j < ARRAY2D_NROWS(nav_.pcvs[i].var); j++) + printArray<double, ARRAY2D_NCOLS(nav_.pcvs[i].var)>("\t\t\t", nav_.pcvs[i].var[j]); + } + + if (nav_.sbssat.nsat == 0) + std::cout << "sbssat: empty\n"; + else + { + std::cout << "sbssat: \n"; + std::cout << "\tiodp: " << nav_.sbssat.iodp << "\n"; + std::cout << "\tnsat: " << nav_.sbssat.nsat << "\n"; + std::cout << "\ttlat: " << nav_.sbssat.tlat << "\n"; + for (int i = 0; i < nav_.sbssat.nsat; i++) + { + std::cout << "\tsat: " << i << "\n"; + std::cout << "\t\tsat " << nav_.sbssat.sat[i].sat << "\n"; + + std::cout << "\t\tfcorr.t0 " << nav_.sbssat.sat[i].fcorr.t0.time << " + "<< nav_.sbssat.sat[i].fcorr.t0.sec << "\n"; + std::cout << "\t\tfcorr.prc " << nav_.sbssat.sat[i].fcorr.prc << "\n"; + std::cout << "\t\tfcorr.rrc " << nav_.sbssat.sat[i].fcorr.rrc << "\n"; + std::cout << "\t\tfcorr.dt " << nav_.sbssat.sat[i].fcorr.dt << "\n"; + std::cout << "\t\tfcorr.iodf " << nav_.sbssat.sat[i].fcorr.iodf << "\n"; + std::cout << "\t\tfcorr.udre " << nav_.sbssat.sat[i].fcorr.udre << "\n"; + std::cout << "\t\tfcorr.ai " << nav_.sbssat.sat[i].fcorr.ai << "\n"; + + std::cout << "\t\tlcorr.daf0 " << nav_.sbssat.sat[i].lcorr.daf0 << "\n"; + std::cout << "\t\tlcorr.daf1 " << nav_.sbssat.sat[i].lcorr.daf1 << "\n"; + std::cout << "\t\tlcorr.iode " << nav_.sbssat.sat[i].lcorr.iode << "\n"; + std::cout << "\t\tlcorr.t0 " << nav_.sbssat.sat[i].lcorr.t0.time << " + "<< nav_.sbssat.sat[i].lcorr.t0.sec << "\n"; + printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dpos)>("\t\tlcorr.dpos: ", nav_.sbssat.sat[i].lcorr.dpos); + printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dvel)>("\t\tlcorr.dvel: ", nav_.sbssat.sat[i].lcorr.dvel); + } } for (int i = 0; i < MAXBAND + 1; i++) { - std::cout << "sbsion: " << i << "\n"; - std::cout << "\tiodi: " << nav_.sbsion[i].iodi << "\n"; - std::cout << "\tnigp: " << nav_.sbsion[i].nigp << "\n"; - for (int j = 0; j < nav_.sbsion[i].nigp; j++) + if (nav_.sbsion[i].nigp == 0) + std::cout << "sbsion: " << i << " empty\n"; + else { - std::cout << "\tigp: " << j << "\n"; - std::cout << "\t\tt0: " << nav_.sbsion[i].igp[j].t0.time << " + "<< nav_.sbsion[i].igp[j].t0.sec << "\n"; - std::cout << "\t\tlat: " << nav_.sbsion[i].igp[j].lat << "\n"; - std::cout << "\t\tlon: " << nav_.sbsion[i].igp[j].lon << "\n"; - std::cout << "\t\tgive: " << nav_.sbsion[i].igp[j].give << "\n"; - std::cout << "\t\tdelay: " << nav_.sbsion[i].igp[j].delay << "\n"; + std::cout << "sbsion: " << i << "\n"; + std::cout << "\tiodi: " << nav_.sbsion[i].iodi << "\n"; + std::cout << "\tnigp: " << nav_.sbsion[i].nigp << "\n"; + for (int j = 0; j < nav_.sbsion[i].nigp; j++) + { + std::cout << "\tigp: " << j << "\n"; + std::cout << "\t\tt0: " << nav_.sbsion[i].igp[j].t0.time << " + "<< nav_.sbsion[i].igp[j].t0.sec << "\n"; + std::cout << "\t\tlat: " << nav_.sbsion[i].igp[j].lat << "\n"; + std::cout << "\t\tlon: " << nav_.sbsion[i].igp[j].lon << "\n"; + std::cout << "\t\tgive: " << nav_.sbsion[i].igp[j].give << "\n"; + std::cout << "\t\tdelay: " << nav_.sbsion[i].igp[j].delay << "\n"; + } } } // TODO: - // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ - // double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ // double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ - // double wlbias[MAXSAT]; /* wide-lane bias (cycle) */ - // double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */ - // char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ - // ********** other not copied nav_t content: *********** // erp_t erp; /* earth rotation parameters */ diff --git a/src/observations.cpp b/src/observations.cpp index a6b856a351de30b8aa0dedd2cecdf0cef6c32f17..37ae10e6bbdc2672346c088560cda165fbde1940 100644 --- a/src/observations.cpp +++ b/src/observations.cpp @@ -373,38 +373,6 @@ std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r, } return filterByElevationSnr(azels, snrmask, elmin, multi_freq); - -// for (int obs_i = 0; obs_i < obs_.size(); obs_i++) -// { -// auto&& obs_sat = getObservationByIdx(obs_i); -// const int& sat_number = obs_sat.sat; -// -// // check elevation -// double elevation = computeSatElevation(x_r, sats.at(sat_number).pos); -// if (elevation < elmin) -// { -// //std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl; -// remove_sats.insert(sat_number); -// continue; -// } -// -// // snr TODO: multifrequency (2nd param and 3rd idx) -// if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1) -// { -// //std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl; -// remove_sats.insert(sat_number); -// } -// } -// -// // remove sats -// // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; -// for (auto sat : remove_sats) -// { -// assert(hasSatellite(sat)); -// removeObservationBySat(sat); -// } -// -// return remove_sats; } std::set<int> Observations::filter(const Satellites& sats, @@ -479,12 +447,12 @@ std::set<int> Observations::filter(const Satellites& sats, // std::cout << "final size: " << obs_.size() << std::endl; } -std::set<int> Observations::filter(const Satellites& sats, - const std::set<int>& discarded_sats, - const std::map<int,Eigen::Vector2d>& azels, - const bool& check_code, - const bool& check_carrier_phase, - const Options& opt) +std::set<int> Observations::filter(const Satellites& sats, + const std::set<int>& discarded_sats, + const Azels& azels, + const bool& check_code, + const bool& check_carrier_phase, + const Options& opt) { return filter(sats, discarded_sats, @@ -497,15 +465,15 @@ std::set<int> Observations::filter(const Satellites& sats, opt.ionoopt == IONOOPT_IFLC or (opt.tdcp.enabled and opt.tdcp.multi_freq)); } -std::set<int> Observations::filter(const Satellites& sats, - const std::set<int>& discarded_sats, - const std::map<int,Eigen::Vector2d>& azels, - const bool& check_code, - const bool& check_carrier_phase, - const int& navsys, - const snrmask_t& snrmask, - const double& elmin, - const bool& multi_freq) +std::set<int> Observations::filter(const Satellites& sats, + const std::set<int>& discarded_sats, + const Azels& azels, + const bool& check_code, + const bool& check_carrier_phase, + const int& navsys, + const snrmask_t& snrmask, + const double& elmin, + const bool& multi_freq) { //std::cout << "filter: initial size: " << obs_.size() << std::endl; // Ephemeris diff --git a/src/range.cpp b/src/range.cpp new file mode 100644 index 0000000000000000000000000000000000000000..09730a8a22eda9dc673cfcc12d5505ec397d6a89 --- /dev/null +++ b/src/range.cpp @@ -0,0 +1,155 @@ +/* + * pseudo_range.cpp + * + * Created on: May 28, 2020 + * Author: joanvallve + */ + +#include "gnss_utils/range.h" + +namespace GnssUtils +{ + +Range::Range() +{ + // TODO Auto-generated constructor stub + +} + +Range::~Range() +{ + // TODO Auto-generated destructor stub +} + +Ranges Range::computeRanges(ObservationsPtr obs, + NavigationPtr nav, + const Satellites& sats, + const Azels& azel, + const Eigen::Vector3d& latlonalt, + const Options& opt) +{ + Ranges ranges; + + double dion,dtrp,vmeas,vion,vtrp,P,lam_L1; + prcopt_t prcopt = opt.getPrcopt(); + + //std::cout << "compute pseudo ranges: \n"; + + for (auto i = 0; i<obs->size(); i++) + { + const obsd_t& obs_i(obs->getObservationByIdx(i)); + int sat = obs_i.sat; + const Satellite& sat_i(sats.at(sat)); + assert(azel.count(sat) != 0 && "azimuth and elevation not provided for this satellite"); + double azel_i[2] = {azel.at(sat)(0),azel.at(sat)(1)}; + + //std::cout << "\tsat " << sat << "..."; + + // initialize with error values + ranges.emplace(sat,Range()); + + /* ------------------- Pseudo range ------------------- */ + /* psudorange with code bias correction */ + //std::cout << "prange...\n"; + if ((P=prange(&obs_i,&nav->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0) + { + //std::cout << " error in prange\n"; + continue; + } + + /* ionospheric corrections */ + //std::cout << "iono...\n"; + //std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n"; + //std::cout << "\tnav: \n"; + //nav->print(); + //std::cout << "\tsat: " << sat << "\n"; + //std::cout << "\tlatlonalt: " << latlonalt.transpose() << "\n"; + //std::cout << "\tazel_i: " << azel_i[0] << " " << azel_i[1] << "\n"; + //std::cout << "\topt.ionoopt: " << opt.ionoopt << "\n"; + if (!ionocorr(obs_i.time,&nav->getNavigation(),sat,latlonalt.data(),azel_i,opt.ionoopt,&dion,&vion)) + { + if (opt.ionoopt == IONOOPT_SBAS) + { + //std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC..."; + if (!ionocorr(obs_i.time,&nav->getNavigation(),sat,latlonalt.data(),azel_i,IONOOPT_BRDC,&dion,&vion)) + { + //std::cout << " error in ionocorr\n"; + continue; + } + } + else + { + //std::cout << " error in ionocorr\n"; + continue; + } + } + /* GPS-L1 -> L1/B1 */ + //std::cout << "iono2...\n"; + if ((lam_L1=nav->getNavigation().lam[sat-1][0])>0.0) + dion*=std::pow(lam_L1/lam_carr[0],2); + + /* tropospheric corrections */ + //std::cout << "tropo...\n"; + if (!tropcorr(obs_i.time,&nav->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp)) + { + //std::cout << " error in tropcorr\n"; + continue; + } + + /* Store in PseudoRange struct */ + //std::cout << "storing\n"; + ranges[sat].sat = sat; + ranges[sat].P = P; + ranges[sat].iono_corr = -dion; + ranges[sat].tropo_corr = -dtrp; + ranges[sat].sat_clock_corr = CLIGHT*sat_i.clock_bias; + + /* pseudorange corrected */ + ranges[sat].P_corrected = ranges[sat].P + + ranges[sat].iono_corr + + ranges[sat].tropo_corr + + ranges[sat].sat_clock_corr; + + /* error variance */ + ranges[sat].P_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp; + + /* ------------------- Carrier phase ------------------- */ + ranges[sat].L = obs_i.L[0]*nav->getNavigation().lam[sat-1][0]; + ranges[sat].L_corrected = ranges[sat].L; + + /* ionospheric corrections */ + if (opt.tdcp.corr_iono) + { + if (opt.ionoopt==IONOOPT_IFLC) + { + // TODO formulation + //ranges[sat].L_corrected = obs_i.L[0]*nav->getNavigation().lam[sat-1][0]; + } + else + ranges[sat].L_corrected -= ranges[sat].iono_corr; + } + + /* tropospheric corrections */ + if (opt.tdcp.corr_tropo) + ranges[sat].L_corrected += ranges[sat].tropo_corr; + + /* sat clock corrections */ + if (opt.tdcp.corr_clock) + ranges[sat].L_corrected += ranges[sat].sat_clock_corr; + + /* carrier phase variance */ + ranges[sat].L_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier; + + //std::cout << std::endl + // << "\t\tprange: " << pranges[sat].prange << std::endl + // << "\t\tvar: " << pranges[sat].var << std::endl + // << "\t\tP: " << P << std::endl + // << "\t\tsat_i.clock_bias: " << sat_i.clock_bias << std::endl + // << "\t\tdion: " << dion << std::endl + // << "\t\tdtrp: " << dtrp << std::endl; + } + + return ranges; +} + +} /* namespace GnssUtils */ diff --git a/src/snapshot.cpp b/src/snapshot.cpp index 5ff3285b5255469cdebeae80102f4ac4bf9f6569..828372362d389cd8fd6d1e123204f6ae33456c99 100644 --- a/src/snapshot.cpp +++ b/src/snapshot.cpp @@ -33,103 +33,11 @@ void Snapshot::computeSatellites(const int& eph_opt) sats_ = GnssUtils::computeSatellites(*obs_, *nav_, eph_opt); } -void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel, - const Eigen::Vector3d& latlonalt, - const Options& opt) +void Snapshot::computeRanges(const Azels& azel, + const Eigen::Vector3d& latlonalt, + const Options& opt) { - assert(pranges_.empty() && "pseudo ranges already computed!"); + assert(!rangesComputed() && "pseudo ranges already computed!"); - double dion,dtrp,vmeas,vion,vtrp,P,lam_L1; - prcopt_t prcopt = opt.getPrcopt(); - - //std::cout << "compute pseudo ranges: \n"; - - for (auto i = 0; i<obs_->size(); i++) - { - const obsd_t& obs_i(obs_->getObservationByIdx(i)); - int sat = obs_i.sat; - const Satellite& sat_i(sats_.at(sat)); - assert(azel.count(sat) != 0 && "azimuth and elevation not provided for this satellite"); - double azel_i[2] = {azel.at(sat)(0),azel.at(sat)(1)}; - - //std::cout << "\tsat " << sat << "..."; - - // initialize with error values - pranges_.emplace(sat,PseudoRange()); - - /* psudorange with code bias correction */ - //std::cout << "prange...\n"; - if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0) - { - //std::cout << " error in prange\n"; - continue; - } - - /* ionospheric corrections */ - //std::cout << "iono...\n"; - //std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n"; - //std::cout << "\tnav: \n"; - //nav_->print(); - //std::cout << "\tsat: " << sat << "\n"; - //std::cout << "\tlatlonalt: " << latlonalt.transpose() << "\n"; - //std::cout << "\tazel_i: " << azel_i[0] << " " << azel_i[1] << "\n"; - //std::cout << "\topt.ionoopt: " << opt.ionoopt << "\n"; - if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i,opt.ionoopt,&dion,&vion)) - { - if (opt.ionoopt == IONOOPT_SBAS) - { - //std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC..."; - if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i,IONOOPT_BRDC,&dion,&vion)) - { - //std::cout << " error in ionocorr\n"; - continue; - } - } - else - { - //std::cout << " error in ionocorr\n"; - continue; - } - } - - /* GPS-L1 -> L1/B1 */ - //std::cout << "iono2...\n"; - if ((lam_L1=nav_->getNavigation().lam[sat-1][0])>0.0) - dion*=std::pow(lam_L1/lam_carr[0],2); - - /* tropospheric corrections */ - //std::cout << "tropo...\n"; - if (!tropcorr(obs_i.time,&nav_->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp)) - { - //std::cout << " error in tropcorr\n"; - continue; - } - - /* Store in PseudoRange struct */ - //std::cout << "storing\n"; - pranges_[sat].sat = sat; - pranges_[sat].p = P; - pranges_[sat].iono_corr = -dion; - pranges_[sat].tropo_corr = -dtrp; - pranges_[sat].sat_clock_corr = CLIGHT*sat_i.clock_bias; - - /* pseudorange corrected */ - pranges_[sat].prange = pranges_[sat].p + - pranges_[sat].iono_corr + - pranges_[sat].tropo_corr + - pranges_[sat].sat_clock_corr; - - /* error variance */ - pranges_[sat].prange_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp; - - // todo: fill L and carrier_range - - //std::cout << std::endl - // << "\t\tprange: " << pranges_[sat].prange << std::endl - // << "\t\tvar: " << pranges_[sat].var << std::endl - // << "\t\tP: " << P << std::endl - // << "\t\tsat_i.clock_bias: " << sat_i.clock_bias << std::endl - // << "\t\tdion: " << dion << std::endl - // << "\t\tdtrp: " << dtrp << std::endl; - } + ranges_ = Range::computeRanges(obs_, nav_, sats_, azel, latlonalt, opt); }