From 0a8d5e4ffd067f4a49b9155e970cf74f4890c71f Mon Sep 17 00:00:00 2001 From: Gastd Date: Thu, 31 Aug 2017 08:57:27 -0300 Subject: [PATCH 01/48] Solve issue glonass FDMA channel in tracking block --- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 5 +++-- .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 3 ++- .../gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc | 5 +++-- 3 files changed, 8 insertions(+), 5 deletions(-) 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 231058ec8..f944d0c33 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 @@ -196,7 +196,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (GLONASS_L1_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; @@ -224,7 +224,8 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + // d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); 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 f22bbc732..2a7a6f3e7 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 @@ -225,7 +225,8 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + // d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); 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 ad7b16cd6..3e008cb46 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 @@ -161,7 +161,7 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (GLONASS_L1_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; @@ -189,7 +189,8 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + // d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); // DLL/PLL filter initialization From fa8cb9dd5c17e4e4aad72a95d52ea9831cbd818d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 23 Aug 2017 19:04:11 +0200 Subject: [PATCH 02/48] Fix building when compilers have a C++ standard below C++11 --- CMakeLists.txt | 30 ------------------- .../volk_gnsssdr/CMakeLists.txt | 2 +- 2 files changed, 1 insertion(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dd63b0089..773af075a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -582,34 +582,6 @@ if(NOT VOLK_GNSSSDR_FOUND) endif(CMAKE_GENERATOR STREQUAL Xcode) endif(OS_IS_MACOSX) - set(C_FLAGS "${CMAKE_C_FLAGS} -std=c11") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - - if(CMAKE_COMPILER_IS_GNUCXX AND NOT WIN32) - if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS "6.1.1") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - else(CMAKE_CXX_COMPILER_VERSION VERSION_LESS "6.1.1") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") - endif(CMAKE_CXX_COMPILER_VERSION VERSION_LESS "6.1.1") - endif(CMAKE_COMPILER_IS_GNUCXX AND NOT WIN32) - - if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - if(OS_IS_LINUX) - if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.5.0") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - else(CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.5.0") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") - endif(CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.5.0") - endif(OS_IS_LINUX) - if(OS_IS_MACOSX) - if(CLANG_VERSION VERSION_LESS "600") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - else(CLANG_VERSION VERSION_LESS "600") - set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") - endif(CLANG_VERSION VERSION_LESS "600") - endif(OS_IS_MACOSX) - endif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - if(CMAKE_CROSSCOMPILING) set(VOLK_GNSSSDR_COMPILER "") else(CMAKE_CROSSCOMPILING) @@ -619,8 +591,6 @@ if(NOT VOLK_GNSSSDR_FOUND) -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/volk_gnsssdr_module/install -DENABLE_STATIC_LIBS=ON -DENABLE_PROFILING=${ENABLE_PROFILING} - -DCMAKE_CXX_FLAGS=${CXX_FLAGS} - -DCMAKE_C_FLAGS=${C_FLAGS} -DCMAKE_INCLUDE_PATH=${Boost_INCLUDE_DIR} -DENABLE_ORC=OFF ${STRIP_VOLK_GNSSSDR_PROFILE} diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt index d6e69819b..8178609a2 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt @@ -27,7 +27,7 @@ enable_language(CXX) enable_language(C) enable_testing() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") add_definitions(-D_GLIBCXX_USE_CXX11_ABI=1) From ed021f043fab534af8829430d36231873c4d1897 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 23 Aug 2017 22:35:22 +0200 Subject: [PATCH 03/48] Merge Gabriel's glonass-sdr branch --- .../glonass_l1_ca_pcps_acquisition.cc | 4 +-- .../adapters/glonass_l1_ca_pcps_acquisition.h | 33 ++++++++++--------- .../glonass_l1_ca_dll_pll_c_aid_tracking.h | 24 +++++++------- .../adapters/glonass_l1_ca_dll_pll_tracking.h | 20 +++++------ ...glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 4 +-- .../glonass_l1_ca_dll_pll_tracking_cc.cc | 4 +-- 6 files changed, 44 insertions(+), 45 deletions(-) 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 2ef2828f1..5d81dc992 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -266,9 +266,9 @@ float GlonassL1CaPcpsAcquisition::calculate_threshold(float pfa) unsigned int ncells = vector_length_ * frequency_bins; double exponent = 1 / static_cast(ncells); double val = pow(1.0 - pfa, exponent); - double lambda = double(vector_length_); + double lambda = static_cast(vector_length_); boost::math::exponential_distribution mydist (lambda); - float threshold = (float)quantile(mydist,val); + float threshold = static_cast(quantile(mydist,val)); return threshold; } 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 2cc43c90f..ff01d7364 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h @@ -29,7 +29,7 @@ public: virtual ~GlonassL1CaPcpsAcquisition(); - std::string role() + inline std::string role() override { return role_; } @@ -37,66 +37,67 @@ public: /*! * \brief Returns "GLONASS_L1_CA_PCPS_Acquisition" */ - std::string implementation() + inline std::string implementation() override { return "GLONASS_L1_CA_PCPS_Acquisition"; } - size_t item_size() + + inline size_t item_size() override { return item_size_; } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and * tracking blocks */ - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; /*! * \brief Set acquisition channel unique ID */ - void set_channel(unsigned int channel); + void set_channel(unsigned int channel) override; /*! * \brief Set statistics threshold of PCPS algorithm */ - void set_threshold(float threshold); + void set_threshold(float threshold) override; /*! * \brief Set maximum Doppler off grid search */ - void set_doppler_max(unsigned int doppler_max); + void set_doppler_max(unsigned int doppler_max) override; /*! * \brief Set Doppler steps for the grid search */ - void set_doppler_step(unsigned int doppler_step); + void set_doppler_step(unsigned int doppler_step) override; /*! * \brief Initializes acquisition algorithm. */ - void init(); + void init() override; /*! * \brief Sets local code for GPS L1/CA PCPS acquisition algorithm. */ - void set_local_code(); + void set_local_code() override; /*! * \brief Returns the maximum peak of grid search */ - signed int mag(); + signed int mag() override; /*! * \brief Restart acquisition algorithm */ - void reset(); + void reset() override; /*! * \brief If state = 1, it forces the block to start acquiring from the first sample diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h index 19a9502b5..3da85c58e 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h @@ -15,7 +15,6 @@ class ConfigurationInterface; class GlonassL1CaDllPllCAidTracking : public TrackingInterface { public: - GlonassL1CaDllPllCAidTracking(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, @@ -23,40 +22,39 @@ public: virtual ~GlonassL1CaDllPllCAidTracking(); - std::string role() + inline std::string role() override { return role_; } //! Returns "GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking" - std::string implementation() + inline std::string implementation() override { return "GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking"; } - size_t item_size() + + inline size_t item_size() override { return item_size_; } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; /*! * \brief Set tracking channel unique ID */ - void set_channel(unsigned int channel); + void set_channel(unsigned int channel) override; /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and tracking blocks */ - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; - - void start_tracking(); + void start_tracking() override; private: glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr tracking_cc; diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h index 46565b2e1..ba476948a 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h @@ -59,39 +59,39 @@ public: virtual ~GlonassL1CaDllPllTracking(); - std::string role() + inline std::string role() override { return role_; } //! Returns "GLONASS_L1_CA_DLL_PLL_Tracking" - std::string implementation() + inline std::string implementation() override { return "GLONASS_L1_CA_DLL_PLL_Tracking"; } - size_t item_size() + inline size_t item_size() override { return item_size_; } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; /*! * \brief Set tracking channel unique ID */ - void set_channel(unsigned int channel); + void set_channel(unsigned int channel) override; /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and tracking blocks */ - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; - void start_tracking(); + void start_tracking() override; private: glonass_l1_ca_dll_pll_tracking_cc_sptr tracking_; 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 f944d0c33..527b0f14e 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 @@ -286,8 +286,8 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __a gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // Block input data and block output stream pointers - const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + const gr_complex* in = reinterpret_cast(input_items[0]); //PRN start block alignment + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); 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 3e008cb46..65e756c2e 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 @@ -270,8 +270,8 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribu double code_error_filt_chips = 0.0; // Block input data and block output stream pointers - const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + const gr_complex* in = reinterpret_cast(input_items[0]); // PRN start block alignment + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); From d4a1dbaf08bf56852897d2a766e1eaac92f831b9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 23 Aug 2017 23:06:56 +0200 Subject: [PATCH 04/48] Replace C-style casts by C++ casts --- .../adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc | 6 ++++++ .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 2 +- .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 4 ++-- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc index 6f9bf340a..4f73fa200 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc @@ -107,6 +107,7 @@ void GlonassL1CaDllPllCAidTracking::start_tracking() } } + /* * Set tracking channel unique ID */ @@ -128,6 +129,7 @@ void GlonassL1CaDllPllCAidTracking::set_channel(unsigned int channel) } } + void GlonassL1CaDllPllCAidTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { if (item_type_.compare("gr_complex") == 0) @@ -144,18 +146,21 @@ void GlonassL1CaDllPllCAidTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchr } } + void GlonassL1CaDllPllCAidTracking::connect(gr::top_block_sptr top_block) { if(top_block) { /* top_block is not null */}; //nothing to connect, now the tracking uses gr_sync_decimator } + void GlonassL1CaDllPllCAidTracking::disconnect(gr::top_block_sptr top_block) { if(top_block) { /* top_block is not null */}; //nothing to disconnect, now the tracking uses gr_sync_decimator } + gr::basic_block_sptr GlonassL1CaDllPllCAidTracking::get_left_block() { if (item_type_.compare("gr_complex") == 0) @@ -173,6 +178,7 @@ gr::basic_block_sptr GlonassL1CaDllPllCAidTracking::get_left_block() } } + gr::basic_block_sptr GlonassL1CaDllPllCAidTracking::get_right_block() { if (item_type_.compare("gr_complex") == 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 527b0f14e..f08edc5a3 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 @@ -286,7 +286,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __a gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // Block input data and block output stream pointers - const gr_complex* in = reinterpret_cast(input_items[0]); //PRN start block alignment + const gr_complex* in = reinterpret_cast(input_items[0]); // PRN start block alignment Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder 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 2a7a6f3e7..e13c48108 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 @@ -290,8 +290,8 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __a gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // Block input data and block output stream pointers - const lv_16sc_t* in = (lv_16sc_t*) input_items[0]; //PRN start block alignment - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + const lv_16sc_t* in = reinterpret_cast(input_items[0]); // PRN start block alignment + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); From 1e59501cb5cea430558fb03f73d8f96cb4530788 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 24 Aug 2017 18:03:23 +0200 Subject: [PATCH 05/48] Fix redefinition of variable --- ...ps_quicksync_ambiguous_acquisition_gsoc2014_test.cc | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc index f059c7a1b..37d6a8813 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc @@ -109,6 +109,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test_msg_rx::GalileoE1PcpsQuic rx_message = 0; } + GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test_msg_rx::~GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test_msg_rx() {} @@ -127,7 +128,6 @@ protected: gnss_synchro = Gnss_Synchro(); init(); } - ~GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test() { } @@ -201,6 +201,7 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::init() Pmd = 0; } + void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::config_1() { gnss_synchro.Channel_ID = 0; @@ -273,6 +274,7 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::config_1() config->set_property("Acquisition.dump", "false"); } + void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::config_2() { gnss_synchro.Channel_ID = 0; @@ -458,12 +460,14 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::config_3() config->set_property("Acquisition.dump", "false"); } + void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::start_queue() { stop = false; ch_thread = boost::thread(&GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::wait_message, this); } + void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::wait_message() { std::chrono::time_point begin, end; @@ -478,7 +482,7 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::wait_message() channel_internal_queue.wait_and_pop(message); end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - begin; + elapsed_seconds = end - begin; mean_acq_time_us += elapsed_seconds.count() * 1e6; @@ -582,7 +586,7 @@ TEST_F(GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test, ConnectAndRun) begin = std::chrono::system_clock::now(); top_block->run(); // Start threads and wait end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - begin; + elapsed_seconds = end - begin; }) << "Failure running the top_block."<< std::endl; std::cout << "Processed " << nsamples << " samples in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; From 8bbe4edc6429bf2150e883d025d30a54c34a309b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 24 Aug 2017 18:09:04 +0200 Subject: [PATCH 06/48] Replace library by and also replace C-style casts by C++ casts. Added/removed some blank lines here and there to match coding style of other tests. --- ...ss_l1_ca_pcps_acquisition_gsoc2017_test.cc | 76 +++++++++++-------- .../glonass_l1_ca_pcps_acquisition_test.cc | 35 ++++----- 2 files changed, 59 insertions(+), 52 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc index a346cf12f..434e37d23 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -72,6 +72,7 @@ GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx::GlonassL1CaPcpsAcquisitionGSoC201 rx_message = 0; } + GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx::~GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx() {} @@ -138,6 +139,7 @@ protected: double Pfa_a; }; + void GlonassL1CaPcpsAcquisitionGSoC2017Test::init() { message = 0; @@ -153,6 +155,7 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::init() Pfa_a = 0; } + void GlonassL1CaPcpsAcquisitionGSoC2017Test::config_1() { gnss_synchro.Channel_ID = 0; @@ -222,6 +225,7 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::config_1() config->set_property("Acquisition.dump", "false"); } + void GlonassL1CaPcpsAcquisitionGSoC2017Test::config_2() { gnss_synchro.Channel_ID = 0; @@ -309,12 +313,14 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::config_2() config->set_property("Acquisition.dump", "false"); } + void GlonassL1CaPcpsAcquisitionGSoC2017Test::start_queue() { stop = false; ch_thread = boost::thread(&GlonassL1CaPcpsAcquisitionGSoC2017Test::wait_message, this); } + void GlonassL1CaPcpsAcquisitionGSoC2017Test::wait_message() { struct timeval tv; @@ -339,6 +345,7 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::wait_message() } } + void GlonassL1CaPcpsAcquisitionGSoC2017Test::process_message() { if (message == 1) @@ -347,7 +354,7 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::process_message() // The term -5 is here to correct the additional delay introduced by the FIR filter // The value 511.0 must be a variable, chips/length - double delay_error_chips = std::abs((double)expected_delay_chips - (double)(gnss_synchro.Acq_delay_samples-5)*511.0/((double)fs_in*1e-3)); + double delay_error_chips = std::abs(static_cast(expected_delay_chips) - (static_cast(gnss_synchro.Acq_delay_samples) - 5.0 ) * 511.0 / (static_cast(fs_in) * 1e-3)); double doppler_error_hz = std::abs(expected_doppler_hz - gnss_synchro.Acq_doppler_hz); mse_delay += std::pow(delay_error_chips, 2); @@ -361,16 +368,16 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::process_message() realization_counter++; - std::cout << "Progress: " << round((float)realization_counter/num_of_realizations*100) << "% \r" << std::flush; + std::cout << "Progress: " << round(static_cast(realization_counter) / static_cast(num_of_realizations) * 100.0) << "% \r" << std::flush; if (realization_counter == num_of_realizations) { mse_delay /= num_of_realizations; mse_doppler /= num_of_realizations; - Pd = (double)correct_estimation_counter / (double)num_of_realizations; - Pfa_a = (double)detection_counter / (double)num_of_realizations; - Pfa_p = (double)(detection_counter - correct_estimation_counter) / (double)num_of_realizations; + Pd = static_cast(correct_estimation_counter) / static_cast(num_of_realizations); + Pfa_a = static_cast(detection_counter) / static_cast(num_of_realizations); + Pfa_p = (static_cast(detection_counter) - static_cast( correct_estimation_counter)) / static_cast(num_of_realizations); mean_acq_time_us /= num_of_realizations; @@ -379,11 +386,13 @@ void GlonassL1CaPcpsAcquisitionGSoC2017Test::process_message() } } + void GlonassL1CaPcpsAcquisitionGSoC2017Test::stop_queue() { stop = true; } + TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, Instantiate) { config_1(); @@ -391,12 +400,12 @@ TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, Instantiate) delete acquisition; } + TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ConnectAndRun) { - int nsamples = floor(fs_in*integration_time_ms*1e-3); - struct timeval tv; - long long int begin = 0; - long long int end = 0; + int nsamples = floor(fs_in * integration_time_ms * 1e-3); + std::chrono::time_point begin, end; + std::chrono::duration elapsed_seconds(0); queue = gr::msg_queue::make(0); top_block = gr::make_top_block("Acquisition test"); @@ -414,18 +423,18 @@ TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ConnectAndRun) }) << "Failure connecting the blocks of acquisition test."<< std::endl; EXPECT_NO_THROW( { - gettimeofday(&tv, NULL); - begin = tv.tv_sec *1e6 + tv.tv_usec; + begin = std::chrono::system_clock::now(); top_block->run(); // Start threads and wait - gettimeofday(&tv, NULL); - end = tv.tv_sec *1e6 + tv.tv_usec; + end = std::chrono::system_clock::now(); + elapsed_seconds = end - begin; }) << "Failure running the top_block."<< std::endl; - std::cout << "Processed " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; + std::cout << "Processed " << nsamples << " samples in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; delete acquisition; } + TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResults) { config_1(); @@ -495,18 +504,18 @@ TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResults) }) << "Failure running the top_block."<< std::endl; if (i == 0) - { - EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS."; - if (message == 1) - { - EXPECT_EQ((unsigned int) 1, correct_estimation_counter) << "Acquisition failure. Incorrect parameters estimation."; - } + { + EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS."; + if (message == 1) + { + EXPECT_EQ(static_cast(1), correct_estimation_counter) << "Acquisition failure. Incorrect parameters estimation."; + } - } + } else if (i == 1) - { - EXPECT_EQ(2, message) << "Acquisition failure. Expected message: 2=ACQ FAIL."; - } + { + EXPECT_EQ(2, message) << "Acquisition failure. Expected message: 2=ACQ FAIL."; + } #ifdef OLD_BOOST ASSERT_NO_THROW( { ch_thread.timed_join(boost::posix_time::seconds(1)); @@ -522,6 +531,7 @@ TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResults) delete acquisition; } + TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResultsProbabilities) { config_2(); @@ -592,15 +602,15 @@ TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResultsProbabilities) }) << "Failure running the top_block."<< std::endl; if (i == 0) - { - std::cout << "Estimated probability of detection = " << Pd << std::endl; - std::cout << "Estimated probability of false alarm (satellite present) = " << Pfa_p << std::endl; - std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl; } + { + std::cout << "Estimated probability of detection = " << Pd << std::endl; + std::cout << "Estimated probability of false alarm (satellite present) = " << Pfa_p << std::endl; + std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl; } else if (i == 1) - { - std::cout << "Estimated probability of false alarm (satellite absent) = " << Pfa_a << std::endl; - std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl; - } + { + std::cout << "Estimated probability of false alarm (satellite absent) = " << Pfa_a << std::endl; + std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl; + } #ifdef OLD_BOOST ASSERT_NO_THROW( { ch_thread.timed_join(boost::posix_time::seconds(1)); diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc index 1a29bda80..902fcdf00 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -119,20 +119,19 @@ void GlonassL1CaPcpsAcquisitionTest::init() } - TEST_F(GlonassL1CaPcpsAcquisitionTest, Instantiate) { init(); boost::shared_ptr acquisition = boost::make_shared(config.get(), "Acquisition", 1, 1); } + TEST_F(GlonassL1CaPcpsAcquisitionTest, ConnectAndRun) { int fs_in = 62314000; int nsamples = 62314; - struct timeval tv; - long long int begin = 0; - long long int end = 0; + std::chrono::time_point begin, end; + std::chrono::duration elapsed_seconds(0); gr::msg_queue::sptr queue = gr::msg_queue::make(0); top_block = gr::make_top_block("Acquisition test"); @@ -151,21 +150,20 @@ TEST_F(GlonassL1CaPcpsAcquisitionTest, ConnectAndRun) }) << "Failure connecting the blocks of acquisition test." << std::endl; EXPECT_NO_THROW( { - gettimeofday(&tv, NULL); - begin = tv.tv_sec * 1000000 + tv.tv_usec; + begin = std::chrono::system_clock::now(); top_block->run(); // Start threads and wait - gettimeofday(&tv, NULL); - end = tv.tv_sec * 1000000 + tv.tv_usec; + end = std::chrono::system_clock::now(); + elapsed_seconds = end - begin; }) << "Failure running the top_block." << std::endl; - std::cout << "Processed " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; + std::cout << "Processed " << nsamples << " samples in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; } + TEST_F(GlonassL1CaPcpsAcquisitionTest, ValidationOfResults) { - struct timeval tv; - long long int begin = 0; - long long int end = 0; + std::chrono::time_point begin, end; + std::chrono::duration elapsed_seconds(0); top_block = gr::make_top_block("Acquisition test"); double expected_delay_samples = 31874; @@ -213,21 +211,20 @@ TEST_F(GlonassL1CaPcpsAcquisitionTest, ValidationOfResults) acquisition->init(); EXPECT_NO_THROW( { - gettimeofday(&tv, NULL); - begin = tv.tv_sec * 1000000 + tv.tv_usec; + begin = std::chrono::system_clock::now(); top_block->run(); // Start threads and wait - gettimeofday(&tv, NULL); - end = tv.tv_sec * 1000000 + tv.tv_usec; + end = std::chrono::system_clock::now(); + elapsed_seconds = end - begin; }) << "Failure running the top_block." << std::endl; unsigned long int nsamples = gnss_synchro.Acq_samplestamp_samples; - std::cout << "Acquired " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; + std::cout << "Acquired " << nsamples << " samples in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; ASSERT_EQ(1, msg_rx->rx_message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS."; double delay_error_samples = std::abs(expected_delay_samples - gnss_synchro.Acq_delay_samples); - float delay_error_chips = (float)(delay_error_samples * 511 / 62316); + float delay_error_chips = static_cast(delay_error_samples) * 511.0 / 62316.0; double doppler_error_hz = std::abs(expected_doppler_hz - gnss_synchro.Acq_doppler_hz); EXPECT_LE(doppler_error_hz, 666) << "Doppler error exceeds the expected value: 666 Hz = 2/(3*integration period)"; From 5530352e8af040819b40414131acbfb6e4cfde10 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 11:42:02 +0200 Subject: [PATCH 07/48] Fix bug in NMEA messages. Fixes #45 Add a new test for the NMEA printer --- src/algorithms/PVT/libs/nmea_printer.cc | 14 ++- src/algorithms/PVT/libs/pvt_solution.cc | 3 + src/algorithms/PVT/libs/pvt_solution.h | 20 +-- src/tests/test_main.cc | 1 + .../pvt/nmea_printer_test.cc | 115 ++++++++++++++++++ 5 files changed, 137 insertions(+), 16 deletions(-) create mode 100644 src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index bb49a1c3a..97ab793a7 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -236,9 +236,10 @@ std::string Nmea_Printer::latitude_to_hm(double lat) out_string.fill('0'); out_string.width(2); out_string << deg; - out_string.width(6); - out_string.precision(4); - out_string << mins; + out_string.width(2); + out_string << static_cast(mins) << "."; + out_string.width(4); + out_string << static_cast((mins - static_cast(static_cast(mins))) * 1e4); if (north == true) { @@ -273,9 +274,10 @@ std::string Nmea_Printer::longitude_to_hm(double longitude) out_string.width(3); out_string.fill('0'); out_string << deg; - out_string.width(6); - out_string.precision(4); - out_string << mins; + out_string.width(2); + out_string << static_cast(mins) << "."; + out_string.width(4); + out_string << static_cast((mins - static_cast(static_cast(mins))) * 1e4); if (east == true) { diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 25ddea211..8b9842411 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -619,6 +619,9 @@ void Pvt_Solution::set_valid_position(bool is_valid) void Pvt_Solution::set_rx_pos(const arma::vec & pos) { d_rx_pos = pos; + d_latitude_d = d_rx_pos(0); + d_longitude_d = d_rx_pos(1); + d_height_m = d_rx_pos(2); } diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index bf503c024..0a7cc451d 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -77,7 +77,7 @@ private: double d_VDOP; double d_TDOP; - int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites + int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites double d_visible_satellites_El[PVT_MAX_CHANNELS] = {}; // Array with the LOS Elevation of the valid satellites double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {}; // Array with the LOS Azimuth of the valid satellites double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; // Array with the LOS Distance of the valid satellites @@ -86,18 +86,18 @@ private: public: Pvt_Solution(); - double get_time_offset_s() const; //!< Get RX time offset [s] - void set_time_offset_s(double offset); //!< Set RX time offset [s] + double get_time_offset_s() const; //!< Get RX time offset [s] + void set_time_offset_s(double offset); //!< Set RX time offset [s] - double get_latitude() const; //!< Get RX position Latitude WGS84 [deg] - double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] - double get_height() const; //!< Get RX position height WGS84 [m] + double get_latitude() const; //!< Get RX position Latitude WGS84 [deg] + double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] + double get_height() const; //!< Get RX position height WGS84 [m] - double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg] - double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg] - double get_avg_height() const; //!< Get RX position averaged height WGS84 [m] + double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg] + double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg] + double get_avg_height() const; //!< Get RX position averaged height WGS84 [m] - void set_rx_pos(const arma::vec & pos); + void set_rx_pos(const arma::vec & pos); //!< Set position: Latitude [deg], longitude [deg], height [m] arma::vec get_rx_pos() const; bool is_valid_position() const; diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index b86bde939..60b33a4d0 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -125,6 +125,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/pvt/rtcm_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rtcm_printer_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc" +#include "unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc" #if EXTRA_TESTS #include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc" diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc new file mode 100644 index 000000000..328b4c998 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -0,0 +1,115 @@ +/*! + * \file nma_printer_test.cc + * \brief Implements Unit Tests for the Nmea_Printer class. + * \author Carles Fernandez-Prades, 2017. cfernandez(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2016 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include +#include +#include "nmea_printer.h" + + +TEST(NmeaPrinterTest, PrintLine) +{ + std::string filename("nmea_test.nmea"); + + std::shared_ptr pvt_solution = std::make_shared(); + + boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), + boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc + pvt_solution->set_position_UTC_time(pt); + + arma::vec pos = {49.27416667, -123.18533333, 0}; + pvt_solution->set_rx_pos(pos); + + pvt_solution->set_valid_position(true); + + ASSERT_NO_THROW( { + std::shared_ptr nmea_printer = std::make_shared(filename, false, ""); + nmea_printer->Print_Nmea_Line(pvt_solution, false); + } ) << "Failure printing NMEA messages."; + + std::ifstream test_file(filename); + std::string line; + std::string GPRMC("$GPRMC"); + if(test_file.is_open()) + { + while(getline (test_file,line)) + { + std::size_t found = line.find(GPRMC); + if (found != std::string::npos) + { + EXPECT_EQ(line, "$GPRMC,225446.000,A,4916.4500,N,12311.1199,W,0.00,0.00,191194,,*1c\r"); + } + } + test_file.close(); + } + EXPECT_EQ(0, remove(filename.c_str())) << "Failure deleting a temporary file."; +} + + + +TEST(NmeaPrinterTest, PrintLineLessthan10min) +{ + std::string filename("nmea_test.nmea"); + + std::shared_ptr pvt_solution = std::make_shared(); + + boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), + boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc + pvt_solution->set_position_UTC_time(pt); + + arma::vec pos = {49.07416667, -123.02527778, 0}; + pvt_solution->set_rx_pos(pos); + + pvt_solution->set_valid_position(true); + + ASSERT_NO_THROW( { + std::shared_ptr nmea_printer = std::make_shared(filename, false, ""); + nmea_printer->Print_Nmea_Line(pvt_solution, false); + } ) << "Failure printing NMEA messages."; + + std::ifstream test_file(filename); + std::string line; + std::string GPRMC("$GPRMC"); + if(test_file.is_open()) + { + while(getline (test_file,line)) + { + std::size_t found = line.find(GPRMC); + if (found != std::string::npos) + { + EXPECT_EQ(line, "$GPRMC,225446.000,A,4904.4500,N,12301.5166,W,0.00,0.00,191194,,*1a\r"); + } + } + test_file.close(); + } + EXPECT_EQ(0, remove(filename.c_str())) << "Failure deleting a temporary file."; +} From bf2e796c1046608c9c062a6f0931b9348d38dd83 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 12:36:00 +0200 Subject: [PATCH 08/48] Fix examples with old configuration notation. Fixes #64" --- conf/gnss-sdr_GPS_L1_USRP_realtime.conf | 125 ++++++------------- conf/gnss-sdr_multisource_Hybrid_ishort.conf | 94 +++++++------- 2 files changed, 82 insertions(+), 137 deletions(-) diff --git a/conf/gnss-sdr_GPS_L1_USRP_realtime.conf b/conf/gnss-sdr_GPS_L1_USRP_realtime.conf index 22584d4e4..5c610f999 100644 --- a/conf/gnss-sdr_GPS_L1_USRP_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_USRP_realtime.conf @@ -177,71 +177,19 @@ Resampler.sample_freq_out=2000000 ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_GPS.count=6 +Channels_1C.count=6 ;#count: Number of available Galileo satellite channels. -Channels_Galileo.count=0 +Channels_1B.count=0 ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver Channels.in_acquisition=1 -;#system: GPS, GLONASS, GALILEO, SBAS or COMPASS -;#if the option is disabled by default is assigned GPS -Channel.system=GPS + ;#signal: ;# "1C" GPS L1 C/A -;# "1P" GPS L1 P -;# "1W" GPS L1 Z-tracking and similar (AS on) -;# "1Y" GPS L1 Y -;# "1M" GPS L1 M -;# "1N" GPS L1 codeless -;# "2C" GPS L2 C/A -;# "2D" GPS L2 L1(C/A)+(P2-P1) semi-codeless ;# "2S" GPS L2 L2C (M) -;# "2L" GPS L2 L2C (L) -;# "2X" GPS L2 L2C (M+L) -;# "2P" GPS L2 P -;# "2W" GPS L2 Z-tracking and similar (AS on) -;# "2Y" GPS L2 Y -;# "2M" GPS GPS L2 M -;# "2N" GPS L2 codeless -;# "5I" GPS L5 I -;# "5Q" GPS L5 Q -;# "5X" GPS L5 I+Q -;# "1C" GLONASS G1 C/A -;# "1P" GLONASS G1 P -;# "2C" GLONASS G2 C/A (Glonass M) -;# "2P" GLONASS G2 P -;# "1A" GALILEO E1 A (PRS) ;# "1B" GALILEO E1 B (I/NAV OS/CS/SoL) -;# "1C" GALILEO E1 C (no data) -;# "1X" GALILEO E1 B+C -;# "1Z" GALILEO E1 A+B+C -;# "5I" GALILEO E5a I (F/NAV OS) -;# "5Q" GALILEO E5a Q (no data) ;# "5X" GALILEO E5a I+Q -;# "7I" GALILEO E5b I -;# "7Q" GALILEO E5b Q -;# "7X" GALILEO E5b I+Q -;# "8I" GALILEO E5 I -;# "8Q" GALILEO E5 Q -;# "8X" GALILEO E5 I+Q -;# "6A" GALILEO E6 A -;# "6B" GALILEO E6 B -;# "6C" GALILEO E6 C -;# "6X" GALILEO E6 B+C -;# "6Z" GALILEO E6 A+B+C -;# "1C" SBAS L1 C/A -;# "5I" SBAS L5 I -;# "5Q" SBAS L5 Q -;# "5X" SBAS L5 I+Q -;# "2I" COMPASS E2 I -;# "2Q" COMPASS E2 Q -;# "2X" COMPASS E2 IQ -;# "7I" COMPASS E5b I -;# "7Q" COMPASS E5b Q -;# "7X" COMPASS E5b IQ -;# "6I" COMPASS E6 I -;# "6Q" COMPASS E6 Q -;# "6X" COMPASS E6 IQ + ;#if the option is disabled by default is assigned "1C" GPS L1 C/A Channel.signal=1C @@ -250,46 +198,45 @@ Channel.signal=1C ;######### CHANNEL 0 CONFIG ############ -Channel0.system=GPS -Channel0.signal=1C +;Channel0.system=GPS +;Channel0.signal=1C ;#satellite: Satellite PRN ID for this channel. Disable this option to random search -Channel0.satellite=11 +;Channel0.satellite=11 ;######### CHANNEL 1 CONFIG ############ -Channel1.system=GPS -Channel1.signal=1C -Channel1.satellite=18 +;Channel1.system=GPS +;Channel1.signal=1C +;Channel1.satellite=18 ;######### ACQUISITION GLOBAL CONFIG ############ ;#dump: Enable or disable the acquisition internal data file logging [true] or [false] -Acquisition_GPS.dump=false +Acquisition_1C.dump=false ;#filename: Log path and filename -Acquisition_GPS.dump_filename=./acq_dump.dat -;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. -Acquisition_GPS.item_type=gr_complex +Acquisition_1C.dump_filename=./acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. +Acquisition_1C.item_type=gr_complex ;#if: Signal intermediate frequency in [Hz] -Acquisition_GPS.if=0 +Acquisition_1C.if=0 ;#sampled_ms: Signal block duration for the acquisition signal detection [ms] -Acquisition_GPS.coherent_integration_time_ms=1 -;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] -Acquisition_GPS.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.coherent_integration_time_ms=1 +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition ;#threshold: Acquisition threshold. It will be ignored if pfa is defined. -Acquisition_GPS.threshold=0.01 +Acquisition_1C.threshold=0.01 ;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] -;Acquisition_GPS.pfa=0.0001 +;Acquisition_1C.pfa=0.0001 ;#doppler_max: Maximum expected Doppler shift [Hz] -Acquisition_GPS.doppler_max=10000 +Acquisition_1C.doppler_max=10000 ;#doppler_max: Doppler step in the grid search [Hz] -Acquisition_GPS.doppler_step=500 +Acquisition_1C.doppler_step=500 ;#bit_transition_flag: Enable or disable a strategy to deal with bit transitions in GPS signals: process two dwells and take maximum test statistics. Only use with implementation: [GPS_L1_CA_PCPS_Acquisition] (should not be used for Galileo_E1_PCPS_Ambiguous_Acquisition]) -Acquisition_GPS.bit_transition_flag=false +Acquisition_1C.bit_transition_flag=false ;#max_dwells: Maximum number of consecutive dwells to be processed. It will be ignored if bit_transition_flag=true -Acquisition_GPS.max_dwells=1 +Acquisition_1C.max_dwells=1 ;######### ACQUISITION CHANNELS CONFIG ###### @@ -299,37 +246,37 @@ Acquisition_GPS.max_dwells=1 ;######### TRACKING GLOBAL CONFIG ############ ;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] -Tracking_GPS.implementation=GPS_L1_CA_DLL_PLL_Tracking +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking ;#item_type: Type and resolution for each of the signal samples. -Tracking_GPS.item_type=gr_complex +Tracking_1C.item_type=gr_complex ;#sampling_frequency: Signal Intermediate Frequency in [Hz] -Tracking_GPS.if=0 +Tracking_1C.if=0 ;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] -Tracking_GPS.dump=false +Tracking_1C.dump=false ;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. -Tracking_GPS.dump_filename=./tracking_ch_ +Tracking_1C.dump_filename=./tracking_ch_ ;#pll_bw_hz: PLL loop filter bandwidth [Hz] -Tracking_GPS.pll_bw_hz=50.0; +Tracking_1C.pll_bw_hz=30.0; ;#dll_bw_hz: DLL loop filter bandwidth [Hz] -Tracking_GPS.dll_bw_hz=2.0; +Tracking_1C.dll_bw_hz=4.0; ;#order: PLL/DLL loop filter order [2] or [3] -Tracking_GPS.order=3; +Tracking_1C.order=3; -;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] -Tracking_GPS.early_late_space_chips=0.5; +;#early_late_space_chips: correlator early-late space [chips] +Tracking_1C.early_late_space_chips=0.5; ;######### TELEMETRY DECODER GPS CONFIG ############ ;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A -TelemetryDecoder_GPS.implementation=GPS_L1_CA_Telemetry_Decoder -TelemetryDecoder_GPS.dump=false +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false ;#decimation factor -TelemetryDecoder_GPS.decimation_factor=1; +TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ ;#implementation: diff --git a/conf/gnss-sdr_multisource_Hybrid_ishort.conf b/conf/gnss-sdr_multisource_Hybrid_ishort.conf index 31f777693..87300194b 100644 --- a/conf/gnss-sdr_multisource_Hybrid_ishort.conf +++ b/conf/gnss-sdr_multisource_Hybrid_ishort.conf @@ -283,14 +283,12 @@ Resampler1.sample_freq_out=4000000 ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_GPS.count=2 +Channels_1C.count=2 ;#count: Number of available Galileo satellite channels. -Channels_Galileo.count=2 +Channels_1B.count=2 ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver Channels.in_acquisition=1 -;#system: GPS, GLONASS, GALILEO, SBAS or COMPASS -;#if the option is disabled by default is assigned GPS -Channel.system=GPS, Galileo + ;# CHANNEL CONNECTION Channel0.RF_channel_ID=0 @@ -305,118 +303,118 @@ Channel.signal=1B ;######### GPS ACQUISITION CONFIG ############ ;#dump: Enable or disable the acquisition internal data file logging [true] or [false] -Acquisition_GPS.dump=false +Acquisition_1C.dump=false ;#filename: Log path and filename -Acquisition_GPS.dump_filename=./acq_dump.dat +Acquisition_1C.dump_filename=./acq_dump.dat ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. -Acquisition_GPS.item_type=gr_complex +Acquisition_1C.item_type=gr_complex ;#if: Signal intermediate frequency in [Hz] -Acquisition_GPS.if=0 +Acquisition_1C.if=0 ;#sampled_ms: Signal block duration for the acquisition signal detection [ms] -Acquisition_GPS.sampled_ms=1 +Acquisition_1C.sampled_ms=1 ;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] -Acquisition_GPS.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition ;#threshold: Acquisition threshold -Acquisition_GPS.threshold=0.0075 +Acquisition_1C.threshold=0.0075 ;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] -;Acquisition_GPS.pfa=0.01 +;Acquisition_1C.pfa=0.01 ;#doppler_max: Maximum expected Doppler shift [Hz] -Acquisition_GPS.doppler_max=10000 +Acquisition_1C.doppler_max=10000 ;#doppler_max: Doppler step in the grid search [Hz] -Acquisition_GPS.doppler_step=500 +Acquisition_1C.doppler_step=500 ;######### GALILEO ACQUISITION CONFIG ############ ;#dump: Enable or disable the acquisition internal data file logging [true] or [false] -Acquisition_Galileo.dump=false +Acquisition_1B.dump=false ;#filename: Log path and filename -Acquisition_Galileo.dump_filename=./acq_dump.dat +Acquisition_1B.dump_filename=./acq_dump.dat ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. -Acquisition_Galileo.item_type=gr_complex +Acquisition_1B.item_type=gr_complex ;#if: Signal intermediate frequency in [Hz] -Acquisition_Galileo.if=0 +Acquisition_1B.if=0 ;#sampled_ms: Signal block duration for the acquisition signal detection [ms] -Acquisition_Galileo.sampled_ms=4 +Acquisition_1B.sampled_ms=4 ;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] -Acquisition_Galileo.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition ;#threshold: Acquisition threshold -;Acquisition_Galileo.threshold=0 +;Acquisition_1B.threshold=0 ;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] -Acquisition_Galileo.pfa=0.0000008 +Acquisition_1B.pfa=0.0000008 ;#doppler_max: Maximum expected Doppler shift [Hz] -Acquisition_Galileo.doppler_max=15000 +Acquisition_1B.doppler_max=15000 ;#doppler_max: Doppler step in the grid search [Hz] -Acquisition_Galileo.doppler_step=125 +Acquisition_1B.doppler_step=125 ;######### TRACKING GPS CONFIG ############ ;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] -Tracking_GPS.implementation=GPS_L1_CA_DLL_PLL_Tracking +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. -Tracking_GPS.item_type=gr_complex +Tracking_1C.item_type=gr_complex ;#sampling_frequency: Signal Intermediate Frequency in [Hz] -Tracking_GPS.if=0 +Tracking_1C.if=0 ;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] -Tracking_GPS.dump=false +Tracking_1C.dump=false ;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. -Tracking_GPS.dump_filename=../data/epl_tracking_ch_ +Tracking_1C.dump_filename=../data/epl_tracking_ch_ ;#pll_bw_hz: PLL loop filter bandwidth [Hz] -Tracking_GPS.pll_bw_hz=45.0; +Tracking_1C.pll_bw_hz=45.0; ;#dll_bw_hz: DLL loop filter bandwidth [Hz] -Tracking_GPS.dll_bw_hz=4.0; +Tracking_1C.dll_bw_hz=4.0; ;#order: PLL/DLL loop filter order [2] or [3] -Tracking_GPS.order=3; +Tracking_1C.order=3; ;######### TRACKING GALILEO CONFIG ############ ;#implementation: Selected tracking algorithm: [Galileo_E1_DLL_PLL_VEML_Tracking] -Tracking_Galileo.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. -Tracking_Galileo.item_type=gr_complex +Tracking_1B.item_type=gr_complex ;#sampling_frequency: Signal Intermediate Frequency in [Hz] -Tracking_Galileo.if=0 +Tracking_1B.if=0 ;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] -Tracking_Galileo.dump=false +Tracking_1B.dump=false ;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. -Tracking_Galileo.dump_filename=../data/veml_tracking_ch_ +Tracking_1B.dump_filename=../data/veml_tracking_ch_ ;#pll_bw_hz: PLL loop filter bandwidth [Hz] -Tracking_Galileo.pll_bw_hz=15.0; +Tracking_1B.pll_bw_hz=15.0; ;#dll_bw_hz: DLL loop filter bandwidth [Hz] -Tracking_Galileo.dll_bw_hz=2.0; +Tracking_1B.dll_bw_hz=2.0; ;#order: PLL/DLL loop filter order [2] or [3] -Tracking_Galileo.order=3; +Tracking_1B.order=3; ;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo -Tracking_Galileo.early_late_space_chips=0.15; +Tracking_1B.early_late_space_chips=0.15; ;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] -Tracking_Galileo.very_early_late_space_chips=0.6; +Tracking_1B.very_early_late_space_chips=0.6; ;######### TELEMETRY DECODER GPS CONFIG ############ ;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A -TelemetryDecoder_GPS.implementation=GPS_L1_CA_Telemetry_Decoder -TelemetryDecoder_GPS.dump=false +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false ;#decimation factor -TelemetryDecoder_GPS.decimation_factor=4; +TelemetryDecoder_1C.decimation_factor=4; ;######### TELEMETRY DECODER GALILEO CONFIG ############ ;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B -TelemetryDecoder_Galileo.implementation=Galileo_E1B_Telemetry_Decoder -TelemetryDecoder_Galileo.dump=false +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ From 12aaa25d5035bb8b1dad6ccc7c94552eb0bded06 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 19:17:12 +0200 Subject: [PATCH 09/48] Do not use LOG(FATAL) since the glog library could have been stripped off --- .../gps_l1_ca_pcps_acquisition_fpga.cc | 25 ++++++-------- .../adapters/osmosdr_signal_source.cc | 32 ++++++++++------- .../adapters/osmosdr_signal_source.h | 2 ++ .../adapters/rtl_tcp_signal_source.cc | 27 +++++++++------ .../adapters/rtl_tcp_signal_source.h | 2 ++ src/main/main.cc | 34 +++++++++++++++++-- ...8ms_ambiguous_acquisition_gsoc2013_test.cc | 2 +- 7 files changed, 82 insertions(+), 42 deletions(-) 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 a16344c3a..a596a7219 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 @@ -32,6 +32,7 @@ */ #include "gps_l1_ca_pcps_acquisition_fpga.h" +#include #include #include #include "GPS_L1_CA.h" @@ -63,29 +64,24 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( DLOG(INFO) << "role " << role; - item_type_ = configuration_->property(role + ".item_type", - default_item_type); + item_type_ = configuration_->property(role + ".item_type", default_item_type); fs_in = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); ifreq = configuration_->property(role + ".if", 0); dump = configuration_->property(role + ".dump", false); doppler_max_ = configuration_->property(role + ".doppler_max", 5000); - sampled_ms = configuration_->property( - role + ".coherent_integration_time_ms", 1); + sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); // note : the FPGA is implemented according to bit transition flag = 0. Setting bit transition flag to 1 has no effect. - bit_transition_flag = configuration_->property( - role + ".bit_transition_flag", false); + bit_transition_flag = configuration_->property(role + ".bit_transition_flag", false); // note : the FPGA is implemented according to use_CFAR_algorithm = 0. Setting use_CFAR_algorithm to 1 has no effect. - use_CFAR_algorithm_flag = configuration_->property( - role + ".use_CFAR_algorithm", false); + use_CFAR_algorithm_flag = configuration_->property(role + ".use_CFAR_algorithm", false); // note : the FPGA does not use the max_dwells variable. max_dwells_ = configuration_->property(role + ".max_dwells", 1); - dump_filename = configuration_->property(role + ".dump_filename", - default_dump_filename); + dump_filename = configuration_->property(role + ".dump_filename", default_dump_filename); //--- Find number of samples per spreading code ------------------------- code_length = round( @@ -104,12 +100,10 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // vector_length_ *= 2; // } - select_queue_Fpga = configuration_->property(role + ".select_queue_Fpga", - 0); + select_queue_Fpga = configuration_->property(role + ".select_queue_Fpga", 0); std::string default_device_name = "/dev/uio0"; - device_name = configuration_->property(role + ".devicename", - default_device_name); + device_name = configuration_->property(role + ".devicename", default_device_name); if (item_type_.compare("cshort") == 0) { @@ -124,7 +118,8 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( } else { - LOG(FATAL) << item_type_ << " FPGA only accepts chsort"; + LOG(WARNING) << "item_type configured to " << item_type_ << "but FPGA implementation only accepts cshort"; + throw std::invalid_argument( "Wrong input_type configuration. Should be cshort" ); } channel_ = 0; diff --git a/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc b/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc index e9d8f1e61..ea8adef3d 100644 --- a/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc +++ b/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc @@ -75,19 +75,7 @@ OsmosdrSignalSource::OsmosdrSignalSource(ConfigurationInterface* configuration, { item_size_ = sizeof(gr_complex); // 1. Make the driver instance - try - { - if (!osmosdr_args_.empty()) - { - std::cout << "OsmoSdr arguments: " << osmosdr_args_ << std::endl; - LOG(INFO) << "OsmoSdr arguments: " << osmosdr_args_; - } - osmosdr_source_ = osmosdr::source::make(osmosdr_args_); - } - catch( const boost::exception & e ) - { - DLOG(FATAL) << "Boost exception: " << boost::diagnostic_information(e); - } + OsmosdrSignalSource::driver_instance(); // 2 set sampling rate osmosdr_source_->set_sample_rate(sample_rate_); @@ -147,6 +135,24 @@ OsmosdrSignalSource::~OsmosdrSignalSource() {} +void OsmosdrSignalSource::driver_instance() +{ + try + { + if (!osmosdr_args_.empty()) + { + std::cout << "OsmoSdr arguments: " << osmosdr_args_ << std::endl; + LOG(INFO) << "OsmoSdr arguments: " << osmosdr_args_; + } + osmosdr_source_ = osmosdr::source::make(osmosdr_args_); + } + catch( const boost::exception & e ) + { + LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e); + throw std::invalid_argument( "Wrong OsmoSdr arguments" ); + } +} + void OsmosdrSignalSource::connect(gr::top_block_sptr top_block) { diff --git a/src/algorithms/signal_source/adapters/osmosdr_signal_source.h b/src/algorithms/signal_source/adapters/osmosdr_signal_source.h index 6687754b5..e60834d01 100644 --- a/src/algorithms/signal_source/adapters/osmosdr_signal_source.h +++ b/src/algorithms/signal_source/adapters/osmosdr_signal_source.h @@ -33,6 +33,7 @@ #ifndef GNSS_SDR_OSMOSDR_SIGNAL_SOURCE_H_ #define GNSS_SDR_OSMOSDR_SIGNAL_SOURCE_H_ +#include #include #include #include @@ -80,6 +81,7 @@ public: gr::basic_block_sptr get_right_block() override; private: + void driver_instance(); std::string role_; // Front-end settings diff --git a/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.cc b/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.cc index 0fbf80700..38da90196 100644 --- a/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.cc +++ b/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.cc @@ -78,16 +78,7 @@ RtlTcpSignalSource::RtlTcpSignalSource(ConfigurationInterface* configuration, { item_size_ = sizeof(gr_complex); // 1. Make the gr block - try - { - std::cout << "Connecting to " << address_ << ":" << port_ << std::endl; - LOG (INFO) << "Connecting to " << address_ << ":" << port_; - signal_source_ = rtl_tcp_make_signal_source_c (address_, port_, flip_iq_); - } - catch( const boost::exception & e ) - { - DLOG(FATAL) << "Boost exception: " << boost::diagnostic_information(e); - } + MakeBlock(); // 2 set sampling rate signal_source_->set_sample_rate(sample_rate_); @@ -145,6 +136,22 @@ RtlTcpSignalSource::~RtlTcpSignalSource() {} +void RtlTcpSignalSource::MakeBlock() +{ + try + { + std::cout << "Connecting to " << address_ << ":" << port_ << std::endl; + LOG (INFO) << "Connecting to " << address_ << ":" << port_; + signal_source_ = rtl_tcp_make_signal_source_c (address_, port_, flip_iq_); + } + catch( const boost::exception & e ) + { + LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e); + throw std::runtime_error( "Failure connecting to the device" ); + } +} + + void RtlTcpSignalSource::connect(gr::top_block_sptr top_block) { if ( samples_ ) diff --git a/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.h b/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.h index 4a2bf88df..283f84ae9 100644 --- a/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.h +++ b/src/algorithms/signal_source/adapters/rtl_tcp_signal_source.h @@ -32,6 +32,7 @@ #ifndef GNSS_SDR_RTL_TCP_SIGNAL_SOURCE_H #define GNSS_SDR_RTL_TCP_SIGNAL_SOURCE_H +#include #include #include #include @@ -81,6 +82,7 @@ public: gr::basic_block_sptr get_right_block() override; private: + void MakeBlock(); std::string role_; // rtl_tcp settings diff --git a/src/main/main.cc b/src/main/main.cc index a641ae92f..ede009fb9 100644 --- a/src/main/main.cc +++ b/src/main/main.cc @@ -39,6 +39,7 @@ #endif #include +#include #include #include #include @@ -140,15 +141,42 @@ int main(int argc, char** argv) } catch(const boost::exception & e) { - LOG(FATAL) << "Boost exception: " << boost::diagnostic_information(e); + if(GOOGLE_STRIP_LOG == 0) + { + LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e); + } + else + { + std::cerr << "Boost exception: " << boost::diagnostic_information(e) << std::endl; + } + google::ShutDownCommandLineFlags(); + return 1; } catch(const std::exception & ex) { - LOG(FATAL) << "STD exception: " << ex.what(); + if(GOOGLE_STRIP_LOG == 0) + { + LOG(WARNING) << "C++ Standard Library exception: " << ex.what(); + } + else + { + std::cerr << "C++ Standard Library exception: " << ex.what() << std::endl; + } + google::ShutDownCommandLineFlags(); + return 1; } catch(...) { - LOG(INFO) << "Unexpected catch"; + if(GOOGLE_STRIP_LOG == 0) + { + LOG(WARNING) << "Unexpected catch. This should not happen."; + } + else + { + std::cerr << "Unexpected catch. This should not happen." << std::endl; + } + google::ShutDownCommandLineFlags(); + return 1; } // report the elapsed time diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc index 25ed62ee3..8ccc7586a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc @@ -359,7 +359,7 @@ void GalileoE1Pcps8msAmbiguousAcquisitionGSoC2013Test::wait_message() } catch( const boost::exception & e ) { - DLOG(FATAL) << "Boost exception: " << boost::diagnostic_information(e); + LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e); } end = std::chrono::system_clock::now(); From fe336fef20c4efec8ee331844cd0a109af036cef Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 22:16:06 +0200 Subject: [PATCH 10/48] Update per_support files --- src/core/libs/supl/asn-rrlp/per_support.c | 678 ++++++++++++---------- src/core/libs/supl/asn-rrlp/per_support.h | 74 ++- 2 files changed, 426 insertions(+), 326 deletions(-) diff --git a/src/core/libs/supl/asn-rrlp/per_support.c b/src/core/libs/supl/asn-rrlp/per_support.c index e8299c730..02dd02094 100644 --- a/src/core/libs/supl/asn-rrlp/per_support.c +++ b/src/core/libs/supl/asn-rrlp/per_support.c @@ -9,26 +9,26 @@ char * per_data_string(asn_per_data_t *pd) { - static char buf[2][32]; - static int n; - n = (n+1) % 2; - snprintf(buf[n], sizeof(buf), - "{m=%d span %+d[%d..%d] (%d)}", - pd->moved, - (((int)pd->buffer) & 0xf), - pd->nboff, pd->nbits, - pd->nbits - pd->nboff); - return buf[n]; + static char buf[2][32]; + static int n; + n = (n+1) % 2; + snprintf(buf[n], sizeof(buf[n]), + "{m=%ld span %+ld[%d..%d] (%d)}", + (long)pd->moved, + (((long)pd->buffer) & 0xf), + (int)pd->nboff, (int)pd->nbits, + (int)(pd->nbits - pd->nboff)); + return buf[n]; } void per_get_undo(asn_per_data_t *pd, int nbits) { - if((ssize_t)pd->nboff < nbits) { - assert((ssize_t)pd->nboff < nbits); - } else { - pd->nboff -= nbits; - pd->moved -= nbits; - } + if((ssize_t)pd->nboff < nbits) { + assert((ssize_t)pd->nboff < nbits); + } else { + pd->nboff -= nbits; + pd->moved -= nbits; + } } /* @@ -36,83 +36,84 @@ per_get_undo(asn_per_data_t *pd, int nbits) { */ int32_t per_get_few_bits(asn_per_data_t *pd, int nbits) { - size_t off; /* Next after last bit offset */ - ssize_t nleft; /* Number of bits left in this stream */ - uint32_t accum; - const uint8_t *buf; + size_t off; /* Next after last bit offset */ + ssize_t nleft; /* Number of bits left in this stream */ + uint32_t accum; + const uint8_t *buf; - if(nbits < 0) - return -1; + if(nbits < 0) + return -1; - nleft = pd->nbits - pd->nboff; - if(nbits > nleft) { - int32_t tailv, vhead; - if(!pd->refill || nbits > 31) return -1; - /* Accumulate unused bytes before refill */ - ASN_DEBUG("Obtain the rest %d bits (want %d)", nleft, nbits); - tailv = per_get_few_bits(pd, nleft); - if(tailv < 0) return -1; - /* Refill (replace pd contents with new data) */ - if(pd->refill(pd)) - return -1; - nbits -= nleft; - vhead = per_get_few_bits(pd, nbits); - /* Combine the rest of previous pd with the head of new one */ - tailv = (tailv << nbits) | vhead; /* Could == -1 */ - return tailv; - } + nleft = pd->nbits - pd->nboff; + if(nbits > nleft) { + int32_t tailv, vhead; + if(!pd->refill || nbits > 31) return -1; + /* Accumulate unused bytes before refill */ + ASN_DEBUG("Obtain the rest %d bits (want %d)", + (int)nleft, (int)nbits); + tailv = per_get_few_bits(pd, nleft); + if(tailv < 0) return -1; + /* Refill (replace pd contents with new data) */ + if(pd->refill(pd)) + return -1; + nbits -= nleft; + vhead = per_get_few_bits(pd, nbits); + /* Combine the rest of previous pd with the head of new one */ + tailv = (tailv << nbits) | vhead; /* Could == -1 */ + return tailv; + } - /* - * Normalize position indicator. - */ - if(pd->nboff >= 8) { - pd->buffer += (pd->nboff >> 3); - pd->nbits -= (pd->nboff & ~0x07); - pd->nboff &= 0x07; - } - pd->moved += nbits; - pd->nboff += nbits; - off = pd->nboff; - buf = pd->buffer; + /* + * Normalize position indicator. + */ + if(pd->nboff >= 8) { + pd->buffer += (pd->nboff >> 3); + pd->nbits -= (pd->nboff & ~0x07); + pd->nboff &= 0x07; + } + pd->moved += nbits; + pd->nboff += nbits; + off = pd->nboff; + buf = pd->buffer; - /* - * Extract specified number of bits. - */ - if(off <= 8) - accum = nbits ? (buf[0]) >> (8 - off) : 0; - else if(off <= 16) - accum = ((buf[0] << 8) + buf[1]) >> (16 - off); - else if(off <= 24) - accum = ((buf[0] << 16) + (buf[1] << 8) + buf[2]) >> (24 - off); - else if(off <= 31) - accum = ((buf[0] << 24) + (buf[1] << 16) - + (buf[2] << 8) + (buf[3])) >> (32 - off); - else if(nbits <= 31) { - asn_per_data_t tpd = *pd; - /* Here are we with our 31-bits limit plus 1..7 bits offset. */ - per_get_undo(&tpd, nbits); - /* The number of available bits in the stream allow - * for the following operations to take place without - * invoking the ->refill() function */ - accum = per_get_few_bits(&tpd, nbits - 24) << 24; - accum |= per_get_few_bits(&tpd, 24); - } else { - per_get_undo(pd, nbits); - return -1; - } + /* + * Extract specified number of bits. + */ + if(off <= 8) + accum = nbits ? (buf[0]) >> (8 - off) : 0; + else if(off <= 16) + accum = ((buf[0] << 8) + buf[1]) >> (16 - off); + else if(off <= 24) + accum = ((buf[0] << 16) + (buf[1] << 8) + buf[2]) >> (24 - off); + else if(off <= 31) + accum = ((buf[0] << 24) + (buf[1] << 16) + + (buf[2] << 8) + (buf[3])) >> (32 - off); + else if(nbits <= 31) { + asn_per_data_t tpd = *pd; + /* Here are we with our 31-bits limit plus 1..7 bits offset. */ + per_get_undo(&tpd, nbits); + /* The number of available bits in the stream allow + * for the following operations to take place without + * invoking the ->refill() function */ + accum = per_get_few_bits(&tpd, nbits - 24) << 24; + accum |= per_get_few_bits(&tpd, 24); + } else { + per_get_undo(pd, nbits); + return -1; + } - accum &= (((uint32_t)1 << nbits) - 1); + accum &= (((uint32_t)1 << nbits) - 1); - ASN_DEBUG(" [PER got %2d<=%2d bits => span %d %+d[%d..%d]:%02x (%d) => 0x%x]", - nbits, nleft, - pd->moved, - (((int)pd->buffer) & 0xf), - pd->nboff, pd->nbits, - pd->buffer[0], - pd->nbits - pd->nboff, - (int)accum); + ASN_DEBUG(" [PER got %2d<=%2d bits => span %d %+ld[%d..%d]:%02x (%d) => 0x%x]", + (int)nbits, (int)nleft, + (int)pd->moved, + (((long)pd->buffer) & 0xf), + (int)pd->nboff, (int)pd->nbits, + ((pd->buffer != NULL)?pd->buffer[0]:0), + (int)(pd->nbits - pd->nboff), + (int)accum); - return accum; + return accum; } /* @@ -120,70 +121,76 @@ per_get_few_bits(asn_per_data_t *pd, int nbits) { */ int per_get_many_bits(asn_per_data_t *pd, uint8_t *dst, int alright, int nbits) { - int32_t value; + int32_t value; - if(alright && (nbits & 7)) { - /* Perform right alignment of a first few bits */ - value = per_get_few_bits(pd, nbits & 0x07); - if(value < 0) return -1; - *dst++ = value; /* value is already right-aligned */ - nbits &= ~7; - } + if(alright && (nbits & 7)) { + /* Perform right alignment of a first few bits */ + value = per_get_few_bits(pd, nbits & 0x07); + if(value < 0) return -1; + *dst++ = value; /* value is already right-aligned */ + nbits &= ~7; + } - while(nbits) { - if(nbits >= 24) { - value = per_get_few_bits(pd, 24); - if(value < 0) return -1; - *(dst++) = value >> 16; - *(dst++) = value >> 8; - *(dst++) = value; - nbits -= 24; - } else { - value = per_get_few_bits(pd, nbits); - if(value < 0) return -1; - if(nbits & 7) { /* implies left alignment */ - value <<= 8 - (nbits & 7), - nbits += 8 - (nbits & 7); - if(nbits > 24) - *dst++ = value >> 24; - } - if(nbits > 16) - *dst++ = value >> 16; - if(nbits > 8) - *dst++ = value >> 8; - *dst++ = value; - break; - } - } + while(nbits) { + if(nbits >= 24) { + value = per_get_few_bits(pd, 24); + if(value < 0) return -1; + *(dst++) = value >> 16; + *(dst++) = value >> 8; + *(dst++) = value; + nbits -= 24; + } else { + value = per_get_few_bits(pd, nbits); + if(value < 0) return -1; + if(nbits & 7) { /* implies left alignment */ + value <<= 8 - (nbits & 7), + nbits += 8 - (nbits & 7); + if(nbits > 24) + *dst++ = value >> 24; + } + if(nbits > 16) + *dst++ = value >> 16; + if(nbits > 8) + *dst++ = value >> 8; + *dst++ = value; + break; + } + } - return 0; + return 0; } /* - * Get the length "n" from the stream. + * X.691-201508 #10.9 General rules for encoding a length determinant. + * Get the optionally constrained length "n" from the stream. */ ssize_t uper_get_length(asn_per_data_t *pd, int ebits, int *repeat) { - ssize_t value; + ssize_t value; - *repeat = 0; + *repeat = 0; - if(ebits >= 0) return per_get_few_bits(pd, ebits); + /* #11.9.4.1 Encoding if constrained (according to effective bits) */ + if(ebits >= 0 && ebits <= 16) { + return per_get_few_bits(pd, ebits); + } - value = per_get_few_bits(pd, 8); - if(value < 0) return -1; - if((value & 128) == 0) /* #10.9.3.6 */ - return (value & 0x7F); - if((value & 64) == 0) { /* #10.9.3.7 */ - value = ((value & 63) << 8) | per_get_few_bits(pd, 8); - if(value < 0) return -1; - return value; - } - value &= 63; /* this is "m" from X.691, #10.9.3.8 */ - if(value < 1 || value > 4) - return -1; - *repeat = 1; - return (16384 * value); + value = per_get_few_bits(pd, 8); + if((value & 0x80) == 0) { /* #11.9.3.6 */ + return (value & 0x7F); + } else if((value & 0x40) == 0) { /* #11.9.3.7 */ + /* bit 8 ... set to 1 and bit 7 ... set to zero */ + value = ((value & 0x3f) << 8) | per_get_few_bits(pd, 8); + return value; /* potential -1 from per_get_few_bits passes through. */ + } else if(value < 0) { + return -1; + } + value &= 0x3f; /* this is "m" from X.691, #11.9.3.8 */ + if(value < 1 || value > 4) { + return -1; /* Prohibited by #11.9.3.8 */ + } + *repeat = 1; + return (16384 * value); } /* @@ -193,21 +200,21 @@ uper_get_length(asn_per_data_t *pd, int ebits, int *repeat) { */ ssize_t uper_get_nslength(asn_per_data_t *pd) { - ssize_t length; + ssize_t length; - ASN_DEBUG("Getting normally small length"); + ASN_DEBUG("Getting normally small length"); - if(per_get_few_bits(pd, 1) == 0) { - length = per_get_few_bits(pd, 6) + 1; - if(length <= 0) return -1; - ASN_DEBUG("l=%d", length); - return length; - } else { - int repeat; - length = uper_get_length(pd, -1, &repeat); - if(length >= 0 && !repeat) return length; - return -1; /* Error, or do not support >16K extensions */ - } + if(per_get_few_bits(pd, 1) == 0) { + length = per_get_few_bits(pd, 6) + 1; + if(length <= 0) return -1; + ASN_DEBUG("l=%d", (int)length); + return length; + } else { + int repeat; + length = uper_get_length(pd, -1, &repeat); + if(length >= 0 && !repeat) return length; + return -1; /* Error, or do not support >16K extensions */ + } } /* @@ -216,135 +223,215 @@ uper_get_nslength(asn_per_data_t *pd) { */ ssize_t uper_get_nsnnwn(asn_per_data_t *pd) { - ssize_t value; + ssize_t value; - value = per_get_few_bits(pd, 7); - if(value & 64) { /* implicit (value < 0) */ - value &= 63; - value <<= 2; - value |= per_get_few_bits(pd, 2); - if(value & 128) /* implicit (value < 0) */ - return -1; - if(value == 0) - return 0; - if(value >= 3) - return -1; - value = per_get_few_bits(pd, 8 * value); - return value; - } + value = per_get_few_bits(pd, 7); + if(value & 64) { /* implicit (value < 0) */ + value &= 63; + value <<= 2; + value |= per_get_few_bits(pd, 2); + if(value & 128) /* implicit (value < 0) */ + return -1; + if(value == 0) + return 0; + if(value >= 3) + return -1; + value = per_get_few_bits(pd, 8 * value); + return value; + } - return value; + return value; } /* - * Put the normally small non-negative whole number. - * X.691, #10.6 + * X.691-11/2008, #11.6 + * Encoding of a normally small non-negative whole number */ int uper_put_nsnnwn(asn_per_outp_t *po, int n) { - int bytes; + int bytes; - if(n <= 63) { - if(n < 0) return -1; - return per_put_few_bits(po, n, 7); - } - if(n < 256) - bytes = 1; - else if(n < 65536) - bytes = 2; - else if(n < 256 * 65536) - bytes = 3; - else - return -1; /* This is not a "normally small" value */ - if(per_put_few_bits(po, bytes, 8)) - return -1; + if(n <= 63) { + if(n < 0) return -1; + return per_put_few_bits(po, n, 7); + } + if(n < 256) + bytes = 1; + else if(n < 65536) + bytes = 2; + else if(n < 256 * 65536) + bytes = 3; + else + return -1; /* This is not a "normally small" value */ + if(per_put_few_bits(po, bytes, 8)) + return -1; - return per_put_few_bits(po, n, 8 * bytes); + return per_put_few_bits(po, n, 8 * bytes); } +/* X.691-2008/11, #11.5.6 -> #11.3 */ +int uper_get_constrained_whole_number(asn_per_data_t *pd, unsigned long *out_value, int nbits) { + unsigned long lhalf; /* Lower half of the number*/ + long half; + + if(nbits <= 31) { + half = per_get_few_bits(pd, nbits); + if(half < 0) return -1; + *out_value = half; + return 0; + } + + if((size_t)nbits > 8 * sizeof(*out_value)) + return -1; /* RANGE */ + + half = per_get_few_bits(pd, 31); + if(half < 0) return -1; + + if(uper_get_constrained_whole_number(pd, &lhalf, nbits - 31)) + return -1; + + *out_value = ((unsigned long)half << (nbits - 31)) | lhalf; + return 0; +} + + +/* X.691-2008/11, #11.5.6 -> #11.3 */ +int uper_put_constrained_whole_number_s(asn_per_outp_t *po, long v, int nbits) { + /* + * Assume signed number can be safely coerced into + * unsigned of the same range. + * The following testing code will likely be optimized out + * by compiler if it is true. + */ + unsigned long uvalue1 = ULONG_MAX; + long svalue = uvalue1; + unsigned long uvalue2 = svalue; + assert(uvalue1 == uvalue2); + return uper_put_constrained_whole_number_u(po, v, nbits); +} + +int uper_put_constrained_whole_number_u(asn_per_outp_t *po, unsigned long v, int nbits) { + if(nbits <= 31) { + return per_put_few_bits(po, v, nbits); + } else { + /* Put higher portion first, followed by lower 31-bit */ + if(uper_put_constrained_whole_number_u(po, v >> 31, nbits - 31)) + return -1; + return per_put_few_bits(po, v, 31); + } +} + +int +per_put_aligned_flush(asn_per_outp_t *po) { + uint32_t unused_bits = (0x7 & (8 - (po->nboff & 0x07))); + size_t complete_bytes = + (po->buffer ? po->buffer - po->tmpspace : 0) + ((po->nboff + 7) >> 3); + + if(unused_bits) { + po->buffer[po->nboff >> 3] &= ~0 << unused_bits; + } + + if(po->outper(po->tmpspace, complete_bytes, po->op_key) < 0) { + return -1; + } else { + po->buffer = po->tmpspace; + po->nboff = 0; + po->nbits = 8 * sizeof(po->tmpspace); + po->flushed_bytes += complete_bytes; + return 0; + } +} + /* * Put a small number of bits (<= 31). */ int per_put_few_bits(asn_per_outp_t *po, uint32_t bits, int obits) { - size_t off; /* Next after last bit offset */ - size_t omsk; /* Existing last byte meaningful bits mask */ - uint8_t *buf; + size_t off; /* Next after last bit offset */ + size_t omsk; /* Existing last byte meaningful bits mask */ + uint8_t *buf; - if(obits <= 0 || obits >= 32) return obits ? -1 : 0; + if(obits <= 0 || obits >= 32) return obits ? -1 : 0; - ASN_DEBUG("[PER put %d bits %x to %p+%d bits]", - obits, (int)bits, po->buffer, po->nboff); + ASN_DEBUG("[PER put %d bits %x to %p+%d bits]", + obits, (int)bits, po->buffer, (int)po->nboff); - /* - * Normalize position indicator. - */ - if(po->nboff >= 8) { - po->buffer += (po->nboff >> 3); - po->nbits -= (po->nboff & ~0x07); - po->nboff &= 0x07; - } + /* + * Normalize position indicator. + */ + if(po->nboff >= 8) { + po->buffer += (po->nboff >> 3); + po->nbits -= (po->nboff & ~0x07); + po->nboff &= 0x07; + } - /* - * Flush whole-bytes output, if necessary. - */ - if(po->nboff + obits > po->nbits) { - int complete_bytes = (po->buffer - po->tmpspace); - ASN_DEBUG("[PER output %d complete + %d]", - complete_bytes, po->flushed_bytes); - if(po->outper(po->tmpspace, complete_bytes, po->op_key) < 0) - return -1; - if(po->nboff) - po->tmpspace[0] = po->buffer[0]; - po->buffer = po->tmpspace; - po->nbits = 8 * sizeof(po->tmpspace); - po->flushed_bytes += complete_bytes; - } + /* + * Flush whole-bytes output, if necessary. + */ + if(po->nboff + obits > po->nbits) { + size_t complete_bytes; + if(!po->buffer) po->buffer = po->tmpspace; + complete_bytes = (po->buffer - po->tmpspace); + ASN_DEBUG("[PER output %ld complete + %ld]", + (long)complete_bytes, (long)po->flushed_bytes); + if(po->outper(po->tmpspace, complete_bytes, po->op_key) < 0) + return -1; + if(po->nboff) + po->tmpspace[0] = po->buffer[0]; + po->buffer = po->tmpspace; + po->nbits = 8 * sizeof(po->tmpspace); + po->flushed_bytes += complete_bytes; + } - /* - * Now, due to sizeof(tmpspace), we are guaranteed large enough space. - */ - buf = po->buffer; - omsk = ~((1 << (8 - po->nboff)) - 1); - off = (po->nboff += obits); + /* + * Now, due to sizeof(tmpspace), we are guaranteed large enough space. + */ + buf = po->buffer; + omsk = ~((1 << (8 - po->nboff)) - 1); + off = (po->nboff + obits); - /* Clear data of debris before meaningful bits */ - bits &= (((uint32_t)1 << obits) - 1); + /* Clear data of debris before meaningful bits */ + bits &= (((uint32_t)1 << obits) - 1); - ASN_DEBUG("[PER out %d %u/%x (t=%d,o=%d) %x&%x=%x]", obits, - (int)bits, (int)bits, - po->nboff - obits, off, buf[0], omsk&0xff, buf[0] & omsk); + ASN_DEBUG("[PER out %d %u/%x (t=%d,o=%d) %x&%x=%x]", obits, + (int)bits, (int)bits, + (int)po->nboff, (int)off, + buf[0], (int)(omsk&0xff), + (int)(buf[0] & omsk)); - if(off <= 8) /* Completely within 1 byte */ - bits <<= (8 - off), - buf[0] = (buf[0] & omsk) | bits; - else if(off <= 16) - bits <<= (16 - off), - buf[0] = (buf[0] & omsk) | (bits >> 8), - buf[1] = bits; - else if(off <= 24) - bits <<= (24 - off), - buf[0] = (buf[0] & omsk) | (bits >> 16), - buf[1] = bits >> 8, - buf[2] = bits; - else if(off <= 31) - bits <<= (32 - off), - buf[0] = (buf[0] & omsk) | (bits >> 24), - buf[1] = bits >> 16, - buf[2] = bits >> 8, - buf[3] = bits; - else { - ASN_DEBUG("->[PER out split %d]", obits); - per_put_few_bits(po, bits >> 8, 24); - per_put_few_bits(po, bits, obits - 24); - ASN_DEBUG("<-[PER out split %d]", obits); - } + if(off <= 8) /* Completely within 1 byte */ + po->nboff = off, + bits <<= (8 - off), + buf[0] = (buf[0] & omsk) | bits; + else if(off <= 16) + po->nboff = off, + bits <<= (16 - off), + buf[0] = (buf[0] & omsk) | (bits >> 8), + buf[1] = bits; + else if(off <= 24) + po->nboff = off, + bits <<= (24 - off), + buf[0] = (buf[0] & omsk) | (bits >> 16), + buf[1] = bits >> 8, + buf[2] = bits; + else if(off <= 31) + po->nboff = off, + bits <<= (32 - off), + buf[0] = (buf[0] & omsk) | (bits >> 24), + buf[1] = bits >> 16, + buf[2] = bits >> 8, + buf[3] = bits; + else { + if(per_put_few_bits(po, bits >> (obits - 24), 24)) return -1; + if(per_put_few_bits(po, bits, obits - 24)) return -1; + } - ASN_DEBUG("[PER out %u/%x => %02x buf+%d]", - (int)bits, (int)bits, buf[0], po->buffer - po->tmpspace); + ASN_DEBUG("[PER out %u/%x => %02x buf+%ld]", + (int)bits, (int)bits, buf[0], + (long)(po->buffer - po->tmpspace)); - return 0; + return 0; } @@ -354,30 +441,30 @@ per_put_few_bits(asn_per_outp_t *po, uint32_t bits, int obits) { int per_put_many_bits(asn_per_outp_t *po, const uint8_t *src, int nbits) { - while(nbits) { - uint32_t value; + while(nbits) { + uint32_t value; - if(nbits >= 24) { - value = (src[0] << 16) | (src[1] << 8) | src[2]; - src += 3; - nbits -= 24; - if(per_put_few_bits(po, value, 24)) - return -1; - } else { - value = src[0]; - if(nbits > 8) - value = (value << 8) | src[1]; - if(nbits > 16) - value = (value << 8) | src[2]; - if(nbits & 0x07) - value >>= (8 - (nbits & 0x07)); - if(per_put_few_bits(po, value, nbits)) - return -1; - break; - } - } + if(nbits >= 24) { + value = (src[0] << 16) | (src[1] << 8) | src[2]; + src += 3; + nbits -= 24; + if(per_put_few_bits(po, value, 24)) + return -1; + } else { + value = src[0]; + if(nbits > 8) + value = (value << 8) | src[1]; + if(nbits > 16) + value = (value << 8) | src[2]; + if(nbits & 0x07) + value >>= (8 - (nbits & 0x07)); + if(per_put_few_bits(po, value, nbits)) + return -1; + break; + } + } - return 0; + return 0; } /* @@ -386,18 +473,18 @@ per_put_many_bits(asn_per_outp_t *po, const uint8_t *src, int nbits) { ssize_t uper_put_length(asn_per_outp_t *po, size_t length) { - if(length <= 127) /* #10.9.3.6 */ - return per_put_few_bits(po, length, 8) - ? -1 : (ssize_t)length; - else if(length < 16384) /* #10.9.3.7 */ - return per_put_few_bits(po, length|0x8000, 16) - ? -1 : (ssize_t)length; + if(length <= 127) /* #10.9.3.6 */ + return per_put_few_bits(po, length, 8) + ? -1 : (ssize_t)length; + else if(length < 16384) /* #10.9.3.7 */ + return per_put_few_bits(po, length|0x8000, 16) + ? -1 : (ssize_t)length; - length >>= 14; - if(length > 4) length = 4; + length >>= 14; + if(length > 4) length = 4; - return per_put_few_bits(po, 0xC0 | length, 8) - ? -1 : (ssize_t)(length << 14); + return per_put_few_bits(po, 0xC0 | length, 8) + ? -1 : (ssize_t)(length << 14); } @@ -409,17 +496,16 @@ uper_put_length(asn_per_outp_t *po, size_t length) { int uper_put_nslength(asn_per_outp_t *po, size_t length) { - if(length <= 64) { - /* #10.9.3.4 */ - if(length == 0) return -1; - return per_put_few_bits(po, length-1, 7) ? -1 : 0; - } else { - if(uper_put_length(po, length) != (ssize_t)length) { - /* This might happen in case of >16K extensions */ - return -1; - } - } + if(length <= 64) { + /* #10.9.3.4 */ + if(length == 0) return -1; + return per_put_few_bits(po, length-1, 7) ? -1 : 0; + } else { + if(uper_put_length(po, length) != (ssize_t)length) { + /* This might happen in case of >16K extensions */ + return -1; + } + } - return 0; + return 0; } - diff --git a/src/core/libs/supl/asn-rrlp/per_support.h b/src/core/libs/supl/asn-rrlp/per_support.h index 7cb1a0ca3..c3e7794de 100644 --- a/src/core/libs/supl/asn-rrlp/per_support.h +++ b/src/core/libs/supl/asn-rrlp/per_support.h @@ -3,10 +3,10 @@ * All rights reserved. * Redistribution and modifications are permitted subject to BSD license. */ -#ifndef _PER_SUPPORT_H_ -#define _PER_SUPPORT_H_ +#ifndef _PER_SUPPORT_H_ +#define _PER_SUPPORT_H_ -#include /* Platform-specific types */ +#include /* Platform-specific types */ #ifdef __cplusplus extern "C" { @@ -15,23 +15,23 @@ extern "C" { /* * Pre-computed PER constraints. */ -typedef struct asn_per_constraint_s { - enum asn_per_constraint_flags { - APC_UNCONSTRAINED = 0x0, /* No PER visible constraints */ - APC_SEMI_CONSTRAINED = 0x1, /* Constrained at "lb" */ - APC_CONSTRAINED = 0x2, /* Fully constrained */ - APC_EXTENSIBLE = 0x4 /* May have extension */ - } flags; - int range_bits; /* Full number of bits in the range */ - int effective_bits; /* Effective bits */ - long lower_bound; /* "lb" value */ - long upper_bound; /* "ub" value */ +typedef const struct asn_per_constraint_s { + enum asn_per_constraint_flags { + APC_UNCONSTRAINED = 0x0, /* No PER visible constraints */ + APC_SEMI_CONSTRAINED = 0x1, /* Constrained at "lb" */ + APC_CONSTRAINED = 0x2, /* Fully constrained */ + APC_EXTENSIBLE = 0x4 /* May have extension */ + } flags; + int range_bits; /* Full number of bits in the range */ + int effective_bits; /* Effective bits */ + long lower_bound; /* "lb" value */ + long upper_bound; /* "ub" value */ } asn_per_constraint_t; -typedef struct asn_per_constraints_s { - asn_per_constraint_t value; - asn_per_constraint_t size; - int (*value2code)(unsigned int value); - int (*code2value)(unsigned int code); +typedef const struct asn_per_constraints_s { + struct asn_per_constraint_s value; + struct asn_per_constraint_s size; + int (*value2code)(unsigned int value); + int (*code2value)(unsigned int code); } asn_per_constraints_t; /* @@ -62,14 +62,14 @@ void per_get_undo(asn_per_data_t *per_data, int get_nbits); * extracted due to EOD or other conditions. */ int per_get_many_bits(asn_per_data_t *pd, uint8_t *dst, int right_align, - int get_nbits); + int get_nbits); /* * Get the length "n" from the Unaligned PER stream. */ ssize_t uper_get_length(asn_per_data_t *pd, - int effective_bound_bits, - int *repeat); + int effective_bound_bits, + int *repeat); /* * Get the normally small length "n". @@ -81,6 +81,9 @@ ssize_t uper_get_nslength(asn_per_data_t *pd); */ ssize_t uper_get_nsnnwn(asn_per_data_t *pd); +/* X.691-2008/11, #11.5.6 */ +int uper_get_constrained_whole_number(asn_per_data_t *pd, unsigned long *v, int nbits); + /* Non-thread-safe debugging function, don't use it */ char *per_data_string(asn_per_data_t *pd); @@ -88,13 +91,13 @@ char *per_data_string(asn_per_data_t *pd); * This structure supports forming PER output. */ typedef struct asn_per_outp_s { - uint8_t *buffer; /* Pointer into the (tmpspace) */ - size_t nboff; /* Bit offset to the meaningful bit */ - size_t nbits; /* Number of bits left in (tmpspace) */ - uint8_t tmpspace[32]; /* Preliminary storage to hold data */ - int (*outper)(const void *data, size_t size, void *op_key); - void *op_key; /* Key for (outper) data callback */ - size_t flushed_bytes; /* Bytes already flushed through (outper) */ + uint8_t *buffer; /* Pointer into the (tmpspace) */ + size_t nboff; /* Bit offset to the meaningful bit */ + size_t nbits; /* Number of bits left in (tmpspace) */ + uint8_t tmpspace[32]; /* Preliminary storage to hold data */ + int (*outper)(const void *data, size_t size, void *op_key); + void *op_key; /* Key for (outper) data callback */ + size_t flushed_bytes; /* Bytes already flushed through (outper) */ } asn_per_outp_t; /* Output a small number of bits (<= 31) */ @@ -103,6 +106,17 @@ int per_put_few_bits(asn_per_outp_t *per_data, uint32_t bits, int obits); /* Output a large number of bits */ int per_put_many_bits(asn_per_outp_t *po, const uint8_t *src, int put_nbits); +/* + * Flush whole bytes (0 or more) through (outper) member. + * The least significant bits which are not used are guaranteed to be set to 0. + * Returns -1 if callback returns -1. Otherwise, 0. + */ +int per_put_aligned_flush(asn_per_outp_t *po); + +/* X.691-2008/11, #11.5 */ +int uper_put_constrained_whole_number_s(asn_per_outp_t *po, long v, int nbits); +int uper_put_constrained_whole_number_u(asn_per_outp_t *po, unsigned long v, int nbits); + /* * Put the length "n" to the Unaligned PER stream. * This function returns the number of units which may be flushed @@ -125,4 +139,4 @@ int uper_put_nsnnwn(asn_per_outp_t *po, int n); } #endif -#endif /* _PER_SUPPORT_H_ */ +#endif /* _PER_SUPPORT_H_ */ From ddee13cfa61fde863a1036e744e8ff161cfee36e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 22:44:49 +0200 Subject: [PATCH 11/48] Update per_support files --- src/core/libs/supl/asn-supl/per_support.c | 678 ++++++++++++---------- src/core/libs/supl/asn-supl/per_support.h | 74 ++- 2 files changed, 426 insertions(+), 326 deletions(-) diff --git a/src/core/libs/supl/asn-supl/per_support.c b/src/core/libs/supl/asn-supl/per_support.c index e8299c730..02dd02094 100644 --- a/src/core/libs/supl/asn-supl/per_support.c +++ b/src/core/libs/supl/asn-supl/per_support.c @@ -9,26 +9,26 @@ char * per_data_string(asn_per_data_t *pd) { - static char buf[2][32]; - static int n; - n = (n+1) % 2; - snprintf(buf[n], sizeof(buf), - "{m=%d span %+d[%d..%d] (%d)}", - pd->moved, - (((int)pd->buffer) & 0xf), - pd->nboff, pd->nbits, - pd->nbits - pd->nboff); - return buf[n]; + static char buf[2][32]; + static int n; + n = (n+1) % 2; + snprintf(buf[n], sizeof(buf[n]), + "{m=%ld span %+ld[%d..%d] (%d)}", + (long)pd->moved, + (((long)pd->buffer) & 0xf), + (int)pd->nboff, (int)pd->nbits, + (int)(pd->nbits - pd->nboff)); + return buf[n]; } void per_get_undo(asn_per_data_t *pd, int nbits) { - if((ssize_t)pd->nboff < nbits) { - assert((ssize_t)pd->nboff < nbits); - } else { - pd->nboff -= nbits; - pd->moved -= nbits; - } + if((ssize_t)pd->nboff < nbits) { + assert((ssize_t)pd->nboff < nbits); + } else { + pd->nboff -= nbits; + pd->moved -= nbits; + } } /* @@ -36,83 +36,84 @@ per_get_undo(asn_per_data_t *pd, int nbits) { */ int32_t per_get_few_bits(asn_per_data_t *pd, int nbits) { - size_t off; /* Next after last bit offset */ - ssize_t nleft; /* Number of bits left in this stream */ - uint32_t accum; - const uint8_t *buf; + size_t off; /* Next after last bit offset */ + ssize_t nleft; /* Number of bits left in this stream */ + uint32_t accum; + const uint8_t *buf; - if(nbits < 0) - return -1; + if(nbits < 0) + return -1; - nleft = pd->nbits - pd->nboff; - if(nbits > nleft) { - int32_t tailv, vhead; - if(!pd->refill || nbits > 31) return -1; - /* Accumulate unused bytes before refill */ - ASN_DEBUG("Obtain the rest %d bits (want %d)", nleft, nbits); - tailv = per_get_few_bits(pd, nleft); - if(tailv < 0) return -1; - /* Refill (replace pd contents with new data) */ - if(pd->refill(pd)) - return -1; - nbits -= nleft; - vhead = per_get_few_bits(pd, nbits); - /* Combine the rest of previous pd with the head of new one */ - tailv = (tailv << nbits) | vhead; /* Could == -1 */ - return tailv; - } + nleft = pd->nbits - pd->nboff; + if(nbits > nleft) { + int32_t tailv, vhead; + if(!pd->refill || nbits > 31) return -1; + /* Accumulate unused bytes before refill */ + ASN_DEBUG("Obtain the rest %d bits (want %d)", + (int)nleft, (int)nbits); + tailv = per_get_few_bits(pd, nleft); + if(tailv < 0) return -1; + /* Refill (replace pd contents with new data) */ + if(pd->refill(pd)) + return -1; + nbits -= nleft; + vhead = per_get_few_bits(pd, nbits); + /* Combine the rest of previous pd with the head of new one */ + tailv = (tailv << nbits) | vhead; /* Could == -1 */ + return tailv; + } - /* - * Normalize position indicator. - */ - if(pd->nboff >= 8) { - pd->buffer += (pd->nboff >> 3); - pd->nbits -= (pd->nboff & ~0x07); - pd->nboff &= 0x07; - } - pd->moved += nbits; - pd->nboff += nbits; - off = pd->nboff; - buf = pd->buffer; + /* + * Normalize position indicator. + */ + if(pd->nboff >= 8) { + pd->buffer += (pd->nboff >> 3); + pd->nbits -= (pd->nboff & ~0x07); + pd->nboff &= 0x07; + } + pd->moved += nbits; + pd->nboff += nbits; + off = pd->nboff; + buf = pd->buffer; - /* - * Extract specified number of bits. - */ - if(off <= 8) - accum = nbits ? (buf[0]) >> (8 - off) : 0; - else if(off <= 16) - accum = ((buf[0] << 8) + buf[1]) >> (16 - off); - else if(off <= 24) - accum = ((buf[0] << 16) + (buf[1] << 8) + buf[2]) >> (24 - off); - else if(off <= 31) - accum = ((buf[0] << 24) + (buf[1] << 16) - + (buf[2] << 8) + (buf[3])) >> (32 - off); - else if(nbits <= 31) { - asn_per_data_t tpd = *pd; - /* Here are we with our 31-bits limit plus 1..7 bits offset. */ - per_get_undo(&tpd, nbits); - /* The number of available bits in the stream allow - * for the following operations to take place without - * invoking the ->refill() function */ - accum = per_get_few_bits(&tpd, nbits - 24) << 24; - accum |= per_get_few_bits(&tpd, 24); - } else { - per_get_undo(pd, nbits); - return -1; - } + /* + * Extract specified number of bits. + */ + if(off <= 8) + accum = nbits ? (buf[0]) >> (8 - off) : 0; + else if(off <= 16) + accum = ((buf[0] << 8) + buf[1]) >> (16 - off); + else if(off <= 24) + accum = ((buf[0] << 16) + (buf[1] << 8) + buf[2]) >> (24 - off); + else if(off <= 31) + accum = ((buf[0] << 24) + (buf[1] << 16) + + (buf[2] << 8) + (buf[3])) >> (32 - off); + else if(nbits <= 31) { + asn_per_data_t tpd = *pd; + /* Here are we with our 31-bits limit plus 1..7 bits offset. */ + per_get_undo(&tpd, nbits); + /* The number of available bits in the stream allow + * for the following operations to take place without + * invoking the ->refill() function */ + accum = per_get_few_bits(&tpd, nbits - 24) << 24; + accum |= per_get_few_bits(&tpd, 24); + } else { + per_get_undo(pd, nbits); + return -1; + } - accum &= (((uint32_t)1 << nbits) - 1); + accum &= (((uint32_t)1 << nbits) - 1); - ASN_DEBUG(" [PER got %2d<=%2d bits => span %d %+d[%d..%d]:%02x (%d) => 0x%x]", - nbits, nleft, - pd->moved, - (((int)pd->buffer) & 0xf), - pd->nboff, pd->nbits, - pd->buffer[0], - pd->nbits - pd->nboff, - (int)accum); + ASN_DEBUG(" [PER got %2d<=%2d bits => span %d %+ld[%d..%d]:%02x (%d) => 0x%x]", + (int)nbits, (int)nleft, + (int)pd->moved, + (((long)pd->buffer) & 0xf), + (int)pd->nboff, (int)pd->nbits, + ((pd->buffer != NULL)?pd->buffer[0]:0), + (int)(pd->nbits - pd->nboff), + (int)accum); - return accum; + return accum; } /* @@ -120,70 +121,76 @@ per_get_few_bits(asn_per_data_t *pd, int nbits) { */ int per_get_many_bits(asn_per_data_t *pd, uint8_t *dst, int alright, int nbits) { - int32_t value; + int32_t value; - if(alright && (nbits & 7)) { - /* Perform right alignment of a first few bits */ - value = per_get_few_bits(pd, nbits & 0x07); - if(value < 0) return -1; - *dst++ = value; /* value is already right-aligned */ - nbits &= ~7; - } + if(alright && (nbits & 7)) { + /* Perform right alignment of a first few bits */ + value = per_get_few_bits(pd, nbits & 0x07); + if(value < 0) return -1; + *dst++ = value; /* value is already right-aligned */ + nbits &= ~7; + } - while(nbits) { - if(nbits >= 24) { - value = per_get_few_bits(pd, 24); - if(value < 0) return -1; - *(dst++) = value >> 16; - *(dst++) = value >> 8; - *(dst++) = value; - nbits -= 24; - } else { - value = per_get_few_bits(pd, nbits); - if(value < 0) return -1; - if(nbits & 7) { /* implies left alignment */ - value <<= 8 - (nbits & 7), - nbits += 8 - (nbits & 7); - if(nbits > 24) - *dst++ = value >> 24; - } - if(nbits > 16) - *dst++ = value >> 16; - if(nbits > 8) - *dst++ = value >> 8; - *dst++ = value; - break; - } - } + while(nbits) { + if(nbits >= 24) { + value = per_get_few_bits(pd, 24); + if(value < 0) return -1; + *(dst++) = value >> 16; + *(dst++) = value >> 8; + *(dst++) = value; + nbits -= 24; + } else { + value = per_get_few_bits(pd, nbits); + if(value < 0) return -1; + if(nbits & 7) { /* implies left alignment */ + value <<= 8 - (nbits & 7), + nbits += 8 - (nbits & 7); + if(nbits > 24) + *dst++ = value >> 24; + } + if(nbits > 16) + *dst++ = value >> 16; + if(nbits > 8) + *dst++ = value >> 8; + *dst++ = value; + break; + } + } - return 0; + return 0; } /* - * Get the length "n" from the stream. + * X.691-201508 #10.9 General rules for encoding a length determinant. + * Get the optionally constrained length "n" from the stream. */ ssize_t uper_get_length(asn_per_data_t *pd, int ebits, int *repeat) { - ssize_t value; + ssize_t value; - *repeat = 0; + *repeat = 0; - if(ebits >= 0) return per_get_few_bits(pd, ebits); + /* #11.9.4.1 Encoding if constrained (according to effective bits) */ + if(ebits >= 0 && ebits <= 16) { + return per_get_few_bits(pd, ebits); + } - value = per_get_few_bits(pd, 8); - if(value < 0) return -1; - if((value & 128) == 0) /* #10.9.3.6 */ - return (value & 0x7F); - if((value & 64) == 0) { /* #10.9.3.7 */ - value = ((value & 63) << 8) | per_get_few_bits(pd, 8); - if(value < 0) return -1; - return value; - } - value &= 63; /* this is "m" from X.691, #10.9.3.8 */ - if(value < 1 || value > 4) - return -1; - *repeat = 1; - return (16384 * value); + value = per_get_few_bits(pd, 8); + if((value & 0x80) == 0) { /* #11.9.3.6 */ + return (value & 0x7F); + } else if((value & 0x40) == 0) { /* #11.9.3.7 */ + /* bit 8 ... set to 1 and bit 7 ... set to zero */ + value = ((value & 0x3f) << 8) | per_get_few_bits(pd, 8); + return value; /* potential -1 from per_get_few_bits passes through. */ + } else if(value < 0) { + return -1; + } + value &= 0x3f; /* this is "m" from X.691, #11.9.3.8 */ + if(value < 1 || value > 4) { + return -1; /* Prohibited by #11.9.3.8 */ + } + *repeat = 1; + return (16384 * value); } /* @@ -193,21 +200,21 @@ uper_get_length(asn_per_data_t *pd, int ebits, int *repeat) { */ ssize_t uper_get_nslength(asn_per_data_t *pd) { - ssize_t length; + ssize_t length; - ASN_DEBUG("Getting normally small length"); + ASN_DEBUG("Getting normally small length"); - if(per_get_few_bits(pd, 1) == 0) { - length = per_get_few_bits(pd, 6) + 1; - if(length <= 0) return -1; - ASN_DEBUG("l=%d", length); - return length; - } else { - int repeat; - length = uper_get_length(pd, -1, &repeat); - if(length >= 0 && !repeat) return length; - return -1; /* Error, or do not support >16K extensions */ - } + if(per_get_few_bits(pd, 1) == 0) { + length = per_get_few_bits(pd, 6) + 1; + if(length <= 0) return -1; + ASN_DEBUG("l=%d", (int)length); + return length; + } else { + int repeat; + length = uper_get_length(pd, -1, &repeat); + if(length >= 0 && !repeat) return length; + return -1; /* Error, or do not support >16K extensions */ + } } /* @@ -216,135 +223,215 @@ uper_get_nslength(asn_per_data_t *pd) { */ ssize_t uper_get_nsnnwn(asn_per_data_t *pd) { - ssize_t value; + ssize_t value; - value = per_get_few_bits(pd, 7); - if(value & 64) { /* implicit (value < 0) */ - value &= 63; - value <<= 2; - value |= per_get_few_bits(pd, 2); - if(value & 128) /* implicit (value < 0) */ - return -1; - if(value == 0) - return 0; - if(value >= 3) - return -1; - value = per_get_few_bits(pd, 8 * value); - return value; - } + value = per_get_few_bits(pd, 7); + if(value & 64) { /* implicit (value < 0) */ + value &= 63; + value <<= 2; + value |= per_get_few_bits(pd, 2); + if(value & 128) /* implicit (value < 0) */ + return -1; + if(value == 0) + return 0; + if(value >= 3) + return -1; + value = per_get_few_bits(pd, 8 * value); + return value; + } - return value; + return value; } /* - * Put the normally small non-negative whole number. - * X.691, #10.6 + * X.691-11/2008, #11.6 + * Encoding of a normally small non-negative whole number */ int uper_put_nsnnwn(asn_per_outp_t *po, int n) { - int bytes; + int bytes; - if(n <= 63) { - if(n < 0) return -1; - return per_put_few_bits(po, n, 7); - } - if(n < 256) - bytes = 1; - else if(n < 65536) - bytes = 2; - else if(n < 256 * 65536) - bytes = 3; - else - return -1; /* This is not a "normally small" value */ - if(per_put_few_bits(po, bytes, 8)) - return -1; + if(n <= 63) { + if(n < 0) return -1; + return per_put_few_bits(po, n, 7); + } + if(n < 256) + bytes = 1; + else if(n < 65536) + bytes = 2; + else if(n < 256 * 65536) + bytes = 3; + else + return -1; /* This is not a "normally small" value */ + if(per_put_few_bits(po, bytes, 8)) + return -1; - return per_put_few_bits(po, n, 8 * bytes); + return per_put_few_bits(po, n, 8 * bytes); } +/* X.691-2008/11, #11.5.6 -> #11.3 */ +int uper_get_constrained_whole_number(asn_per_data_t *pd, unsigned long *out_value, int nbits) { + unsigned long lhalf; /* Lower half of the number*/ + long half; + + if(nbits <= 31) { + half = per_get_few_bits(pd, nbits); + if(half < 0) return -1; + *out_value = half; + return 0; + } + + if((size_t)nbits > 8 * sizeof(*out_value)) + return -1; /* RANGE */ + + half = per_get_few_bits(pd, 31); + if(half < 0) return -1; + + if(uper_get_constrained_whole_number(pd, &lhalf, nbits - 31)) + return -1; + + *out_value = ((unsigned long)half << (nbits - 31)) | lhalf; + return 0; +} + + +/* X.691-2008/11, #11.5.6 -> #11.3 */ +int uper_put_constrained_whole_number_s(asn_per_outp_t *po, long v, int nbits) { + /* + * Assume signed number can be safely coerced into + * unsigned of the same range. + * The following testing code will likely be optimized out + * by compiler if it is true. + */ + unsigned long uvalue1 = ULONG_MAX; + long svalue = uvalue1; + unsigned long uvalue2 = svalue; + assert(uvalue1 == uvalue2); + return uper_put_constrained_whole_number_u(po, v, nbits); +} + +int uper_put_constrained_whole_number_u(asn_per_outp_t *po, unsigned long v, int nbits) { + if(nbits <= 31) { + return per_put_few_bits(po, v, nbits); + } else { + /* Put higher portion first, followed by lower 31-bit */ + if(uper_put_constrained_whole_number_u(po, v >> 31, nbits - 31)) + return -1; + return per_put_few_bits(po, v, 31); + } +} + +int +per_put_aligned_flush(asn_per_outp_t *po) { + uint32_t unused_bits = (0x7 & (8 - (po->nboff & 0x07))); + size_t complete_bytes = + (po->buffer ? po->buffer - po->tmpspace : 0) + ((po->nboff + 7) >> 3); + + if(unused_bits) { + po->buffer[po->nboff >> 3] &= ~0 << unused_bits; + } + + if(po->outper(po->tmpspace, complete_bytes, po->op_key) < 0) { + return -1; + } else { + po->buffer = po->tmpspace; + po->nboff = 0; + po->nbits = 8 * sizeof(po->tmpspace); + po->flushed_bytes += complete_bytes; + return 0; + } +} + /* * Put a small number of bits (<= 31). */ int per_put_few_bits(asn_per_outp_t *po, uint32_t bits, int obits) { - size_t off; /* Next after last bit offset */ - size_t omsk; /* Existing last byte meaningful bits mask */ - uint8_t *buf; + size_t off; /* Next after last bit offset */ + size_t omsk; /* Existing last byte meaningful bits mask */ + uint8_t *buf; - if(obits <= 0 || obits >= 32) return obits ? -1 : 0; + if(obits <= 0 || obits >= 32) return obits ? -1 : 0; - ASN_DEBUG("[PER put %d bits %x to %p+%d bits]", - obits, (int)bits, po->buffer, po->nboff); + ASN_DEBUG("[PER put %d bits %x to %p+%d bits]", + obits, (int)bits, po->buffer, (int)po->nboff); - /* - * Normalize position indicator. - */ - if(po->nboff >= 8) { - po->buffer += (po->nboff >> 3); - po->nbits -= (po->nboff & ~0x07); - po->nboff &= 0x07; - } + /* + * Normalize position indicator. + */ + if(po->nboff >= 8) { + po->buffer += (po->nboff >> 3); + po->nbits -= (po->nboff & ~0x07); + po->nboff &= 0x07; + } - /* - * Flush whole-bytes output, if necessary. - */ - if(po->nboff + obits > po->nbits) { - int complete_bytes = (po->buffer - po->tmpspace); - ASN_DEBUG("[PER output %d complete + %d]", - complete_bytes, po->flushed_bytes); - if(po->outper(po->tmpspace, complete_bytes, po->op_key) < 0) - return -1; - if(po->nboff) - po->tmpspace[0] = po->buffer[0]; - po->buffer = po->tmpspace; - po->nbits = 8 * sizeof(po->tmpspace); - po->flushed_bytes += complete_bytes; - } + /* + * Flush whole-bytes output, if necessary. + */ + if(po->nboff + obits > po->nbits) { + size_t complete_bytes; + if(!po->buffer) po->buffer = po->tmpspace; + complete_bytes = (po->buffer - po->tmpspace); + ASN_DEBUG("[PER output %ld complete + %ld]", + (long)complete_bytes, (long)po->flushed_bytes); + if(po->outper(po->tmpspace, complete_bytes, po->op_key) < 0) + return -1; + if(po->nboff) + po->tmpspace[0] = po->buffer[0]; + po->buffer = po->tmpspace; + po->nbits = 8 * sizeof(po->tmpspace); + po->flushed_bytes += complete_bytes; + } - /* - * Now, due to sizeof(tmpspace), we are guaranteed large enough space. - */ - buf = po->buffer; - omsk = ~((1 << (8 - po->nboff)) - 1); - off = (po->nboff += obits); + /* + * Now, due to sizeof(tmpspace), we are guaranteed large enough space. + */ + buf = po->buffer; + omsk = ~((1 << (8 - po->nboff)) - 1); + off = (po->nboff + obits); - /* Clear data of debris before meaningful bits */ - bits &= (((uint32_t)1 << obits) - 1); + /* Clear data of debris before meaningful bits */ + bits &= (((uint32_t)1 << obits) - 1); - ASN_DEBUG("[PER out %d %u/%x (t=%d,o=%d) %x&%x=%x]", obits, - (int)bits, (int)bits, - po->nboff - obits, off, buf[0], omsk&0xff, buf[0] & omsk); + ASN_DEBUG("[PER out %d %u/%x (t=%d,o=%d) %x&%x=%x]", obits, + (int)bits, (int)bits, + (int)po->nboff, (int)off, + buf[0], (int)(omsk&0xff), + (int)(buf[0] & omsk)); - if(off <= 8) /* Completely within 1 byte */ - bits <<= (8 - off), - buf[0] = (buf[0] & omsk) | bits; - else if(off <= 16) - bits <<= (16 - off), - buf[0] = (buf[0] & omsk) | (bits >> 8), - buf[1] = bits; - else if(off <= 24) - bits <<= (24 - off), - buf[0] = (buf[0] & omsk) | (bits >> 16), - buf[1] = bits >> 8, - buf[2] = bits; - else if(off <= 31) - bits <<= (32 - off), - buf[0] = (buf[0] & omsk) | (bits >> 24), - buf[1] = bits >> 16, - buf[2] = bits >> 8, - buf[3] = bits; - else { - ASN_DEBUG("->[PER out split %d]", obits); - per_put_few_bits(po, bits >> 8, 24); - per_put_few_bits(po, bits, obits - 24); - ASN_DEBUG("<-[PER out split %d]", obits); - } + if(off <= 8) /* Completely within 1 byte */ + po->nboff = off, + bits <<= (8 - off), + buf[0] = (buf[0] & omsk) | bits; + else if(off <= 16) + po->nboff = off, + bits <<= (16 - off), + buf[0] = (buf[0] & omsk) | (bits >> 8), + buf[1] = bits; + else if(off <= 24) + po->nboff = off, + bits <<= (24 - off), + buf[0] = (buf[0] & omsk) | (bits >> 16), + buf[1] = bits >> 8, + buf[2] = bits; + else if(off <= 31) + po->nboff = off, + bits <<= (32 - off), + buf[0] = (buf[0] & omsk) | (bits >> 24), + buf[1] = bits >> 16, + buf[2] = bits >> 8, + buf[3] = bits; + else { + if(per_put_few_bits(po, bits >> (obits - 24), 24)) return -1; + if(per_put_few_bits(po, bits, obits - 24)) return -1; + } - ASN_DEBUG("[PER out %u/%x => %02x buf+%d]", - (int)bits, (int)bits, buf[0], po->buffer - po->tmpspace); + ASN_DEBUG("[PER out %u/%x => %02x buf+%ld]", + (int)bits, (int)bits, buf[0], + (long)(po->buffer - po->tmpspace)); - return 0; + return 0; } @@ -354,30 +441,30 @@ per_put_few_bits(asn_per_outp_t *po, uint32_t bits, int obits) { int per_put_many_bits(asn_per_outp_t *po, const uint8_t *src, int nbits) { - while(nbits) { - uint32_t value; + while(nbits) { + uint32_t value; - if(nbits >= 24) { - value = (src[0] << 16) | (src[1] << 8) | src[2]; - src += 3; - nbits -= 24; - if(per_put_few_bits(po, value, 24)) - return -1; - } else { - value = src[0]; - if(nbits > 8) - value = (value << 8) | src[1]; - if(nbits > 16) - value = (value << 8) | src[2]; - if(nbits & 0x07) - value >>= (8 - (nbits & 0x07)); - if(per_put_few_bits(po, value, nbits)) - return -1; - break; - } - } + if(nbits >= 24) { + value = (src[0] << 16) | (src[1] << 8) | src[2]; + src += 3; + nbits -= 24; + if(per_put_few_bits(po, value, 24)) + return -1; + } else { + value = src[0]; + if(nbits > 8) + value = (value << 8) | src[1]; + if(nbits > 16) + value = (value << 8) | src[2]; + if(nbits & 0x07) + value >>= (8 - (nbits & 0x07)); + if(per_put_few_bits(po, value, nbits)) + return -1; + break; + } + } - return 0; + return 0; } /* @@ -386,18 +473,18 @@ per_put_many_bits(asn_per_outp_t *po, const uint8_t *src, int nbits) { ssize_t uper_put_length(asn_per_outp_t *po, size_t length) { - if(length <= 127) /* #10.9.3.6 */ - return per_put_few_bits(po, length, 8) - ? -1 : (ssize_t)length; - else if(length < 16384) /* #10.9.3.7 */ - return per_put_few_bits(po, length|0x8000, 16) - ? -1 : (ssize_t)length; + if(length <= 127) /* #10.9.3.6 */ + return per_put_few_bits(po, length, 8) + ? -1 : (ssize_t)length; + else if(length < 16384) /* #10.9.3.7 */ + return per_put_few_bits(po, length|0x8000, 16) + ? -1 : (ssize_t)length; - length >>= 14; - if(length > 4) length = 4; + length >>= 14; + if(length > 4) length = 4; - return per_put_few_bits(po, 0xC0 | length, 8) - ? -1 : (ssize_t)(length << 14); + return per_put_few_bits(po, 0xC0 | length, 8) + ? -1 : (ssize_t)(length << 14); } @@ -409,17 +496,16 @@ uper_put_length(asn_per_outp_t *po, size_t length) { int uper_put_nslength(asn_per_outp_t *po, size_t length) { - if(length <= 64) { - /* #10.9.3.4 */ - if(length == 0) return -1; - return per_put_few_bits(po, length-1, 7) ? -1 : 0; - } else { - if(uper_put_length(po, length) != (ssize_t)length) { - /* This might happen in case of >16K extensions */ - return -1; - } - } + if(length <= 64) { + /* #10.9.3.4 */ + if(length == 0) return -1; + return per_put_few_bits(po, length-1, 7) ? -1 : 0; + } else { + if(uper_put_length(po, length) != (ssize_t)length) { + /* This might happen in case of >16K extensions */ + return -1; + } + } - return 0; + return 0; } - diff --git a/src/core/libs/supl/asn-supl/per_support.h b/src/core/libs/supl/asn-supl/per_support.h index 7cb1a0ca3..c3e7794de 100644 --- a/src/core/libs/supl/asn-supl/per_support.h +++ b/src/core/libs/supl/asn-supl/per_support.h @@ -3,10 +3,10 @@ * All rights reserved. * Redistribution and modifications are permitted subject to BSD license. */ -#ifndef _PER_SUPPORT_H_ -#define _PER_SUPPORT_H_ +#ifndef _PER_SUPPORT_H_ +#define _PER_SUPPORT_H_ -#include /* Platform-specific types */ +#include /* Platform-specific types */ #ifdef __cplusplus extern "C" { @@ -15,23 +15,23 @@ extern "C" { /* * Pre-computed PER constraints. */ -typedef struct asn_per_constraint_s { - enum asn_per_constraint_flags { - APC_UNCONSTRAINED = 0x0, /* No PER visible constraints */ - APC_SEMI_CONSTRAINED = 0x1, /* Constrained at "lb" */ - APC_CONSTRAINED = 0x2, /* Fully constrained */ - APC_EXTENSIBLE = 0x4 /* May have extension */ - } flags; - int range_bits; /* Full number of bits in the range */ - int effective_bits; /* Effective bits */ - long lower_bound; /* "lb" value */ - long upper_bound; /* "ub" value */ +typedef const struct asn_per_constraint_s { + enum asn_per_constraint_flags { + APC_UNCONSTRAINED = 0x0, /* No PER visible constraints */ + APC_SEMI_CONSTRAINED = 0x1, /* Constrained at "lb" */ + APC_CONSTRAINED = 0x2, /* Fully constrained */ + APC_EXTENSIBLE = 0x4 /* May have extension */ + } flags; + int range_bits; /* Full number of bits in the range */ + int effective_bits; /* Effective bits */ + long lower_bound; /* "lb" value */ + long upper_bound; /* "ub" value */ } asn_per_constraint_t; -typedef struct asn_per_constraints_s { - asn_per_constraint_t value; - asn_per_constraint_t size; - int (*value2code)(unsigned int value); - int (*code2value)(unsigned int code); +typedef const struct asn_per_constraints_s { + struct asn_per_constraint_s value; + struct asn_per_constraint_s size; + int (*value2code)(unsigned int value); + int (*code2value)(unsigned int code); } asn_per_constraints_t; /* @@ -62,14 +62,14 @@ void per_get_undo(asn_per_data_t *per_data, int get_nbits); * extracted due to EOD or other conditions. */ int per_get_many_bits(asn_per_data_t *pd, uint8_t *dst, int right_align, - int get_nbits); + int get_nbits); /* * Get the length "n" from the Unaligned PER stream. */ ssize_t uper_get_length(asn_per_data_t *pd, - int effective_bound_bits, - int *repeat); + int effective_bound_bits, + int *repeat); /* * Get the normally small length "n". @@ -81,6 +81,9 @@ ssize_t uper_get_nslength(asn_per_data_t *pd); */ ssize_t uper_get_nsnnwn(asn_per_data_t *pd); +/* X.691-2008/11, #11.5.6 */ +int uper_get_constrained_whole_number(asn_per_data_t *pd, unsigned long *v, int nbits); + /* Non-thread-safe debugging function, don't use it */ char *per_data_string(asn_per_data_t *pd); @@ -88,13 +91,13 @@ char *per_data_string(asn_per_data_t *pd); * This structure supports forming PER output. */ typedef struct asn_per_outp_s { - uint8_t *buffer; /* Pointer into the (tmpspace) */ - size_t nboff; /* Bit offset to the meaningful bit */ - size_t nbits; /* Number of bits left in (tmpspace) */ - uint8_t tmpspace[32]; /* Preliminary storage to hold data */ - int (*outper)(const void *data, size_t size, void *op_key); - void *op_key; /* Key for (outper) data callback */ - size_t flushed_bytes; /* Bytes already flushed through (outper) */ + uint8_t *buffer; /* Pointer into the (tmpspace) */ + size_t nboff; /* Bit offset to the meaningful bit */ + size_t nbits; /* Number of bits left in (tmpspace) */ + uint8_t tmpspace[32]; /* Preliminary storage to hold data */ + int (*outper)(const void *data, size_t size, void *op_key); + void *op_key; /* Key for (outper) data callback */ + size_t flushed_bytes; /* Bytes already flushed through (outper) */ } asn_per_outp_t; /* Output a small number of bits (<= 31) */ @@ -103,6 +106,17 @@ int per_put_few_bits(asn_per_outp_t *per_data, uint32_t bits, int obits); /* Output a large number of bits */ int per_put_many_bits(asn_per_outp_t *po, const uint8_t *src, int put_nbits); +/* + * Flush whole bytes (0 or more) through (outper) member. + * The least significant bits which are not used are guaranteed to be set to 0. + * Returns -1 if callback returns -1. Otherwise, 0. + */ +int per_put_aligned_flush(asn_per_outp_t *po); + +/* X.691-2008/11, #11.5 */ +int uper_put_constrained_whole_number_s(asn_per_outp_t *po, long v, int nbits); +int uper_put_constrained_whole_number_u(asn_per_outp_t *po, unsigned long v, int nbits); + /* * Put the length "n" to the Unaligned PER stream. * This function returns the number of units which may be flushed @@ -125,4 +139,4 @@ int uper_put_nsnnwn(asn_per_outp_t *po, int n); } #endif -#endif /* _PER_SUPPORT_H_ */ +#endif /* _PER_SUPPORT_H_ */ From 108ffc7537819a5e34fe27074482dd263584d83c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 23:38:44 +0200 Subject: [PATCH 12/48] Remove warning --- src/core/libs/supl/asn-rrlp/converter-sample.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/libs/supl/asn-rrlp/converter-sample.c b/src/core/libs/supl/asn-rrlp/converter-sample.c index 0a682a275..13baad2e7 100644 --- a/src/core/libs/supl/asn-rrlp/converter-sample.c +++ b/src/core/libs/supl/asn-rrlp/converter-sample.c @@ -384,7 +384,7 @@ buffer_dump() { (long)DynamicBuffer.length - 1, (long)8 - DynamicBuffer.unbits); } else { - fprintf(stderr, " %d\n", DynamicBuffer.length); + fprintf(stderr, " %ld\n", DynamicBuffer.length); } } From 6a4a2db4bfbc00d847a94ec39af0ce9617bb528a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 23:42:36 +0200 Subject: [PATCH 13/48] Remove warning --- src/core/libs/supl/supl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/libs/supl/supl.c b/src/core/libs/supl/supl.c index 1ab843ead..6a06bb252 100644 --- a/src/core/libs/supl/supl.c +++ b/src/core/libs/supl/supl.c @@ -217,7 +217,7 @@ int EXPORT supl_decode_rrlp(supl_ulp_t *ulp_pdu, PDU_t **ret_rrlp) { int EXPORT supl_server_connect(supl_ctx_t *ctx, char *server) { int err; - const SSL_METHOD *meth; + SSL_METHOD *meth; SSLeay_add_ssl_algorithms(); // meth = TLSv1_client_method(); From b9b0bf1ef87dd8182a72b7b0c8aaa5fec31ac403 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 25 Aug 2017 23:51:11 +0200 Subject: [PATCH 14/48] Remove warning --- src/core/libs/supl/asn-rrlp/constr_SET_OF.c | 2 +- src/core/libs/supl/asn-supl/constr_SET_OF.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/core/libs/supl/asn-rrlp/constr_SET_OF.c b/src/core/libs/supl/asn-rrlp/constr_SET_OF.c index 11eac57a2..0bc891abf 100644 --- a/src/core/libs/supl/asn-rrlp/constr_SET_OF.c +++ b/src/core/libs/supl/asn-rrlp/constr_SET_OF.c @@ -915,7 +915,7 @@ SET_OF_decode_uper(asn_codec_ctx_t *opt_codec_ctx, asn_TYPE_descriptor_t *td, nelems = uper_get_length(pd, ct ? ct->effective_bits : -1, &repeat); ASN_DEBUG("Got to decode %d elements (eff %d)", - (int)nelems, (int)ct ? ct->effective_bits : -1); + (int)nelems, (long)ct ? ct->effective_bits : -1); if(nelems < 0) _ASN_DECODE_STARVED; } diff --git a/src/core/libs/supl/asn-supl/constr_SET_OF.c b/src/core/libs/supl/asn-supl/constr_SET_OF.c index 11eac57a2..0bc891abf 100644 --- a/src/core/libs/supl/asn-supl/constr_SET_OF.c +++ b/src/core/libs/supl/asn-supl/constr_SET_OF.c @@ -915,7 +915,7 @@ SET_OF_decode_uper(asn_codec_ctx_t *opt_codec_ctx, asn_TYPE_descriptor_t *td, nelems = uper_get_length(pd, ct ? ct->effective_bits : -1, &repeat); ASN_DEBUG("Got to decode %d elements (eff %d)", - (int)nelems, (int)ct ? ct->effective_bits : -1); + (int)nelems, (long)ct ? ct->effective_bits : -1); if(nelems < 0) _ASN_DECODE_STARVED; } From 8bb188a8df9dfd90f140201e5bec56ca056c99cf Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 26 Aug 2017 00:06:18 +0200 Subject: [PATCH 15/48] Remove warning --- src/core/libs/supl/asn-rrlp/NativeInteger.c | 6 +++--- src/core/libs/supl/asn-supl/NativeInteger.c | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/core/libs/supl/asn-rrlp/NativeInteger.c b/src/core/libs/supl/asn-rrlp/NativeInteger.c index abdb71a8c..bf3939e88 100644 --- a/src/core/libs/supl/asn-rrlp/NativeInteger.c +++ b/src/core/libs/supl/asn-rrlp/NativeInteger.c @@ -107,7 +107,7 @@ NativeInteger_decode_ber(asn_codec_ctx_t *opt_codec_ctx, tmp.size = length; if((specs&&specs->field_unsigned) - ? asn_INTEGER2ulong(&tmp, &l) + ? asn_INTEGER2ulong(&tmp, (unsigned long*)&l) : asn_INTEGER2long(&tmp, &l)) { rval.code = RC_FAIL; rval.consumed = 0; @@ -187,7 +187,7 @@ NativeInteger_decode_xer(asn_codec_ctx_t *opt_codec_ctx, if(rval.code == RC_OK) { long l; if((specs&&specs->field_unsigned) - ? asn_INTEGER2ulong(&st, &l) + ? asn_INTEGER2ulong(&st, (unsigned long*)&l) : asn_INTEGER2long(&st, &l)) { rval.code = RC_FAIL; rval.consumed = 0; @@ -255,7 +255,7 @@ NativeInteger_decode_uper(asn_codec_ctx_t *opt_codec_ctx, &tmpintptr, pd); if(rval.code == RC_OK) { if((specs&&specs->field_unsigned) - ? asn_INTEGER2ulong(&tmpint, native) + ? asn_INTEGER2ulong(&tmpint, (unsigned long*)native) : asn_INTEGER2long(&tmpint, native)) rval.code = RC_FAIL; else diff --git a/src/core/libs/supl/asn-supl/NativeInteger.c b/src/core/libs/supl/asn-supl/NativeInteger.c index abdb71a8c..76140a22e 100644 --- a/src/core/libs/supl/asn-supl/NativeInteger.c +++ b/src/core/libs/supl/asn-supl/NativeInteger.c @@ -107,7 +107,7 @@ NativeInteger_decode_ber(asn_codec_ctx_t *opt_codec_ctx, tmp.size = length; if((specs&&specs->field_unsigned) - ? asn_INTEGER2ulong(&tmp, &l) + ? asn_INTEGER2ulong(&tmp, (unsigned long *)&l) : asn_INTEGER2long(&tmp, &l)) { rval.code = RC_FAIL; rval.consumed = 0; @@ -187,7 +187,7 @@ NativeInteger_decode_xer(asn_codec_ctx_t *opt_codec_ctx, if(rval.code == RC_OK) { long l; if((specs&&specs->field_unsigned) - ? asn_INTEGER2ulong(&st, &l) + ? asn_INTEGER2ulong(&st, (unsigned long *)&l) : asn_INTEGER2long(&st, &l)) { rval.code = RC_FAIL; rval.consumed = 0; @@ -255,7 +255,7 @@ NativeInteger_decode_uper(asn_codec_ctx_t *opt_codec_ctx, &tmpintptr, pd); if(rval.code == RC_OK) { if((specs&&specs->field_unsigned) - ? asn_INTEGER2ulong(&tmpint, native) + ? asn_INTEGER2ulong(&tmpint, (unsigned long *)native) : asn_INTEGER2long(&tmpint, native)) rval.code = RC_FAIL; else From e60a59160b3b086d6986edb958c9ad7f9b2109ec Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 26 Aug 2017 00:22:12 +0200 Subject: [PATCH 16/48] Remove annoying warning in macOS (deprecated-declarations) --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 773af075a..265e79ec5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1452,6 +1452,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") else(CLANG_VERSION VERSION_LESS "600") set(MY_CXX_FLAGS "${MY_CXX_FLAGS} -std=c++14") endif(CLANG_VERSION VERSION_LESS "600") + set(MY_CXX_FLAGS "${MY_CXX_FLAGS} -Wno-deprecated-declarations") endif(OS_IS_MACOSX) if(CMAKE_BUILD_TYPE MATCHES "Release") From 7ae4a794f6c3eb156babdbff911f0063142ed5db Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 26 Aug 2017 10:07:18 +0200 Subject: [PATCH 17/48] Avoid signed overflow --- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 65 ++++++++++----------- 1 file changed, 31 insertions(+), 34 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 850fa37ae..6af5e200d 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -1544,49 +1544,46 @@ int ddmat(rtk_t *rtk, double *D) for (f = 0, k = na;fx[i] == 0.0||!test_sys(rtk->ssat[i-k].sys,m)|| - !rtk->ssat[i-k].vsat[f]) + for (i = k;ix[i] == 0.0 || !test_sys(rtk->ssat[i-k].sys, m) || + if (rtk->x[i] == 0.0 || !test_sys(rtk->ssat[i-k].sys, m) || !rtk->ssat[i-k].vsat[f] || !rtk->ssat[i-k].half[f]) - { -#endif - continue; + { + continue; + } + if (rtk->ssat[i-k].lock[f]>0 && !(rtk->ssat[i-k].slip[f]&2) && + rtk->ssat[i-k].azel[1] >= rtk->opt.elmaskar && !nofix) + { + rtk->ssat[i-k].fix[f] = 2; /* fix */ + break; + } + else rtk->ssat[i-k].fix[f] = 1; } - if (rtk->ssat[i-k].lock[f]>0 && !(rtk->ssat[i-k].slip[f]&2) && - rtk->ssat[i-k].azel[1] >= rtk->opt.elmaskar && !nofix) + for (j = k;jssat[i-k].fix[f] = 2; /* fix */ - break; + if (i == j || rtk->x[j] == 0.0 || !test_sys(rtk->ssat[j-k].sys, m) || + !rtk->ssat[j-k].vsat[f]) + { + continue; + } + if (rtk->ssat[j-k].lock[f]>0 && !(rtk->ssat[j-k].slip[f]&2) && + rtk->ssat[i-k].vsat[f] && + rtk->ssat[j-k].azel[1] >= rtk->opt.elmaskar && !nofix) + { + D[i+(na+nb)*nx] = 1.0; + D[j+(na+nb)*nx] = -1.0; + nb++; + rtk->ssat[j-k].fix[f] = 2; /* fix */ + } + else rtk->ssat[j-k].fix[f] = 1; } - else rtk->ssat[i-k].fix[f] = 1; - } - for (j = k;jx[j] == 0.0 || !test_sys(rtk->ssat[j-k].sys, m) || - !rtk->ssat[j-k].vsat[f]) - { - continue; - } - if (rtk->ssat[j-k].lock[f]>0 && !(rtk->ssat[j-k].slip[f]&2) && - rtk->ssat[i-k].vsat[f] && - rtk->ssat[j-k].azel[1] >= rtk->opt.elmaskar && !nofix) - { - D[i+(na+nb)*nx] = 1.0; - D[j+(na+nb)*nx] = -1.0; - nb++; - rtk->ssat[j-k].fix[f] = 2; /* fix */ - } - else rtk->ssat[j-k].fix[f] = 1; } } } - trace(5, "D=\n"); tracemat(5, D, nx, na+nb, 2, 0); - return nb; + trace(5, "D=\n"); tracemat(5, D, nx, na+nb, 2, 0); + return nb; } From 3d5a55276b2d6d5aa077614c4ae7cf272527facc Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Thu, 25 May 2017 22:15:59 -0600 Subject: [PATCH 18/48] Adding configuration file for GLONASS processing --- conf/gnss-sdr_GLONASS_L1_ibyte.conf | 103 ++++++++++++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 conf/gnss-sdr_GLONASS_L1_ibyte.conf diff --git a/conf/gnss-sdr_GLONASS_L1_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_ibyte.conf new file mode 100644 index 000000000..0b16fa3d8 --- /dev/null +++ b/conf/gnss-sdr_GLONASS_L1_ibyte.conf @@ -0,0 +1,103 @@ +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. +GNSS-SDR.internal_fs_hz=6625000 + +;######### CONTROL_THREAD CONFIG ############ +ControlThread.wait_for_flowgraph=false + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/home/dmiralles/Documents/GSOC/GSOC2017/gnss-sdr/data/dmirallesNT1065_L2_20160831_fs6625e6_60e3_schar_1H.bin +SignalSource.item_type=ibyte +SignalSource.sampling_frequency=6625000 +SignalSource.freq=1602000000 +SignalSource.samples=0 +SignalSource.repeat=false +SignalSource.sample_type=iq +SignalSource.seconds_to_skip=0 +SignalSource.dump=false +SignalSource.dump_filename=../data/signal_source.dat +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner + +;DataTypeAdapter.implementation=Ishort_To_Complex +DataTypeAdapter.implementation=Ibyte_To_Complex +InputFilter.implementation=Pass_Through +;InputFilter.input_item_type=gr_complex +;InputFilter.output_item_type=gr_complex +InputFilter.item_type=gr_complex +;Resampler.implementation=Pass_Through +;Resampler.item_type=gr_complex +Resampler.implementation=Pass_Through +;Resampler.sample_freq_in=4000000 +;Resampler.sample_freq_out=2000000 +;Resampler.item_type=gr_complex +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1R.count=8 ;Assuming here that identifier `1r=R` defines GLONASS SP signals +Channels.in_acquisition=1 +Channel.signal=1R +;Channel.item_type=cshort + + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1R.dump=false +Acquisition_1R.dump_filename=./acq_dump.dat +Acquisition_1R.item_type=cshort +Acquisition_1R.if=0 +Acquisition_1R.sampled_ms=1 +Acquisition_1R.implementation=GLONASS_L1_CA_PCPS_Acquisition +Acquisition_1R.threshold=0.008 +;Acquisition_1C.pfa=0.000001 +Acquisition_1R.doppler_max=10000 +Acquisition_1R.doppler_step=250 +Acquisition_1R.tong_init_val=2 +Acquisition_1R.tong_max_val=10 +Acquisition_1R.tong_max_dwells=20 + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_1R.implementation=GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking +Tracking_1R.item_type=cshort +Tracking_1R.if=0 +Tracking_1R.dump=false +Tracking_1R.dump_filename=../data/epl_tracking_ch_ +Tracking_1R.pll_bw_hz=40.0; +Tracking_1R.dll_bw_hz=4.0; +Tracking_1R.order=3; + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1R.implementation=GLONASS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1R.dump=false +TelemetryDecoder_1R.decimation_factor=1; + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=Hybrid_Observables +Observables.dump=false +Observables.dump_filename=./observables.dat + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100 +PVT.display_rate_ms=500 +PVT.dump_filename=./PVT +PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; +PVT.flag_nmea_tty_port=false; +PVT.nmea_dump_devname=/dev/pts/4 +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.dump=false From f8f3574090c255641c5de869868d93de68fc579f Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 6 Jun 2017 07:27:48 -0700 Subject: [PATCH 19/48] gnav: Adding GLONASS GNAV Ephemeris, Almanac and UTC Model Generates code for GLONASS GNAV Ephemeris, Almanac and UTC Model with their respective decoding position indexes for string decoding and message parsing. Starts developing of satellite position computation based on its ephemeris and almanac information --- src/core/system_parameters/GLONASS_L1_CA.h | 176 +++++ .../system_parameters/glonass_gnav_almanac.cc | 57 ++ .../system_parameters/glonass_gnav_almanac.h | 70 ++ .../glonass_gnav_ephemeris.cc | 600 ++++++++++++++++++ .../glonass_gnav_ephemeris.h | 170 +++++ .../glonass_gnav_utc_model.cc | 48 ++ .../glonass_gnav_utc_model.h | 75 +++ 7 files changed, 1196 insertions(+) create mode 100644 src/core/system_parameters/GLONASS_L1_CA.h create mode 100644 src/core/system_parameters/glonass_gnav_almanac.cc create mode 100644 src/core/system_parameters/glonass_gnav_almanac.h create mode 100644 src/core/system_parameters/glonass_gnav_ephemeris.cc create mode 100644 src/core/system_parameters/glonass_gnav_ephemeris.h create mode 100644 src/core/system_parameters/glonass_gnav_utc_model.cc create mode 100644 src/core/system_parameters/glonass_gnav_utc_model.h diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h new file mode 100644 index 000000000..d3c234801 --- /dev/null +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -0,0 +1,176 @@ +/*! + * \file GLONASS_L1_CA.h + * \brief Defines system parameters for GLONASS L1 C/A signal and NAV data + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_L1_CA_H_ +#define GNSS_SDR_GLONASS_L1_CA_H_ + +#include +#include // std::pair +#include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" + +// Physical constants +const double GLONASS_C_m_s = SPEED_OF_LIGHT; //!< The speed of light, [m/s] +const double GLONASS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] +const double GLONASS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E +const double GLONASS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200E +const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s] +const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [km^3/s^2] +const double GLONASS_fM_a = 0.35e9; //!< Gravitational constant of atmosphere [m^3/s^2] +const double GLONASS_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m] +const double GLONASS_FLATTENING = 1/29825784; //!< Flattening parameter +const double GLONASS_GRAVITY = 97803284; //!< Equatorial acceleration of gravity [mGal] +const double GLONASS_GRAVITY_CORRECTION = 0.87; //!< Correction to acceleration of gravity at sea-level due to Atmosphere[мGal] +const double GLONASS_J2 = 1082625.75e-9; //!< Second zonal harmonic of the geopotential +const double GLONASS_J4 = -2370.89e-9; //!> STRING_ID({{2,4}}); +const std::vector> KX({{78,8}}); +//STRING 1 +const std::vector> P1({{8,2}}); +const std::vector> T_K({{10,12}}); +const std::vector> X_N_DOT ({{22,24}}); +const std::vector> X_N_DOT_DOT ({{46,5}}); +const std::vector> X_N({{51,27}}); + +//STRING 2 +const std::vector> B_N({{6,3}}); +const std::vector> P2({{9,1}}); +const std::vector> T_B({{10,7}}); +const std::vector> Y_N_DOT ({{22,24}}); +const std::vector> Y_N_DOT_DOT ({{46,5}}); +const std::vector> Y_N({{51,27}}); + +//STRING 3 +const std::vector> P3({{6,1}}); +const std::vector> GAMMA_N({{7,11}}); +const std::vector> P({{19,2}}); +const std::vector> L_N({{21,1}}); +const std::vector> Z_N_DOT ({{22,24}}); +const std::vector> Z_N_DOT_DOT ({{46,5}}); +const std::vector> Z_N({{51,27}}); + +// STRING 4 +const std::vector> TAU_N({{6,22}}); +const std::vector> DELTA_TAU_N({{28,5}}); +const std::vector> E_N({{33,5}}); +const std::vector> P4 ({{52,1}}); +const std::vector> F_T ({{53,4}}); +const std::vector> N_T({{60,11}}); +const std::vector> N({{71,5}}); +const std::vector> M({{76,2}}); + +// STRING 5 +const std::vector> N_A({{6,11}}); +const std::vector> TAU_C({{17,32}}); +const std::vector> N_4({{50,5}}); +const std::vector> TAU_GPS({{55,22}}); +const std::vector> L_N({{77,1}}); + +// STRING 6, 8, 10, 12, 14 +const std::vector> C_N({{6,1}}); +const std::vector> M_N_A({{7,2}}); +const std::vector> n_A({{9,5}}); +const std::vector> TAU_N_A({{14,10}}); +const std::vector> LAMBDA_N_A({{24,21}}); +const std::vector> DELTA_I_N_A({{45,18}}); +const std::vector> EPSILON_N_A({{63,15}}); + +//STRING 7, 9, 11, 13, 15 +const std::vector> OMEGA_N_A({{6,16}}); +const std::vector> T_LAMBDA_N_A({{22,21}}); +const std::vector> DELTA_T_N_A({{43,22}}); +const std::vector> DELTA_T_DOT_N_A({{65,7}}); +const std::vector> H_N_A({{72,5}}); +const std::vector> L_N({{77,1}}); + + +#endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc new file mode 100644 index 000000000..45174cabb --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -0,0 +1,57 @@ +/*! + * \file glonass_gnav_almanac.cc + * \brief Interface of a GLONASS GNAV ALMANAC storage + * + * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * \author Damian Miralles , 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "glonass_gnav_almanac.h" + +Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() +{ + int i_satellite_freq_channel = 0; + double d_tau_c = 0.0; + double d_tau_gps = 0.0; + double d_N_4 = 0.0; + double d_N_A = 0.0; + double d_n_A = 0.0; + double d_H_n_A = 0.0; + double d_lambda_n_A = 0.0; + double d_t_lambda_n_A = 0.0; + double d_Delta_i_n_A = 0.0; + double d_Delta_T_n_A = 0.0; + double d_Delta_T_n_A_dot = 0.0; + double d_epsilon_n_A = 0.0; + double d_omega_n_A = 0.0; + double d_M_n_A = 0.0; + double d_B1 = 0.0; + double d_B2 = 0.0; + double d_KP = 0.0; + double d_tau_n_A = 0.0; + double d_C_n_A = 0.0; +} diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h new file mode 100644 index 000000000..3e13802fc --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -0,0 +1,70 @@ +/*! + * \file glonass_gnav_almanac.h + * \brief Interface of a GLONASS GNAV ALMANAC storage + * \author Damian Miralles, 2017. dmiralles2009@gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_ALMANAC_H_ +#define GNSS_SDR_GLONASS_ALMANAC_H_ + + +/*! + * \brief This class is a storage for the GLONASS SV ALMANAC data as described in IS-GPS-200E + * \todo Add proper links for GLONASS ICD + * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + */ +class Glonass_Gnav_Almanac +{ +public: + int i_satellite_freq_channel; //!< SV PRN NUMBER + double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] + double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] + double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] + double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days] + double d_n_A; //!< Conventional number of satellite within GLONASS space segment [dimensionless] + double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] + double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] + double d_t_lambda_n_A; //!< Time of first ascending node passage [s] + double d_Delta_i_n_A //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] + double d_Delta_T_n_A; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period] + double d_Delta_T_n_A_dot; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2] + double d_epsilon_n_A; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless] + double d_omega_n_A; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles] + double d_M_n_A; //!< Type of satellite n_A [dimensionless] + double d_B1; //!< Coefficient to determine DeltaUT1 [s] + double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd] + double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless] + double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] + double d_C_n_A; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + /*! + * Default constructor + */ + Glonass_Gnav_Almanac(); +}; + +#endif diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc new file mode 100644 index 000000000..a6e6436c2 --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -0,0 +1,600 @@ +/*! + * \file gps_ephemeris.cc + * \brief Interface of a GPS EPHEMERIS storage and orbital model functions + * + * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * \author Javier Arribas, 2013. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "gps_ephemeris.h" +#include +#include "GLONASS_L1_CA.h" +#include "gnss_satellite.h" + +Gps_Ephemeris::Gps_Ephemeris() +{ + i_satellite_freq_channel = 0; + d_m = 0.0; //!< String number within frame [dimensionless] + d_t_k = 0.0; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] + d_t_b = 0.0; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] + d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless] + d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] + d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + // satellite positions + d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + // Satellite acceleration + d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + d_B_n = 0.0; //!< Health flag [dimensionless] + d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] + d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] + d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] + d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation + d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] + d_E_n = 0.0; //!< Characterises "age" of a current information [days] + d_P_1 = 0.0; //!< Flag of the immediate data updating. + d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + d_l_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + + // clock terms derived from ephemeris data + d_satClkDrift = 0.0; //!< GLONASS clock error + d_dtr = 0.0; +} + + +void Glonass_GNAV_Ephemeris::gravitational_perturbations() +{ + double T; + double sigma_days; + double tau_prime; + double Omega_moon; + double q_moon; + double q_sun; + double Tau_11; double Tau_12; + double Eta_11; double Eta_12; + double xi_11; double xi_12; + double xi_star; + double eta_star; + double zeta_star; + double E_moon; + double E_sun; + double xi_11; + double xi_12; + double eta_11; + double eta_12; + double zeta_11; + double zeta_12; + + double sin_theta_moon; + double cos_theta_moon; + double theta_moon; + double xi_moon_e; + double eta_moon_e; + double zeta_moon_e; + + double sin_theta_sun; + double cos_theta_sun; + double theta_sun; + double xi_sun_e; + double eta_sun_e; + double zeta_sun_e; + + double r_moon_e; + double mu_bar_moon; + double x_bar_moon; + double y_bar_moon; + double Delta_o_moon; + double mu_bar_sun; + double x_bar_sun; + double y_bar_sun; + double Delta_o_sun; + + + // half_day) + { + corrTime = time - 2.0 * half_day; + } + else if (time < -half_day) + { + corrTime = time + 2.0 * half_day; + } + return corrTime; +} + + +// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. +double Glonass_GNAV_Ephemeris::sv_clock_drift(double transmitTime, double timeCorrUTC) +{ + double dt; + dt = check_t(transmitTime - d_t_b); + d_satClkDrift = -(d_tau_n + timeCorrUTC - d_gamma_n * dt); + //Correct satellite group delay and missing relativistic term here + //d_satClkDrift-=d_TGD; + + return d_satClkDrift; +} + + +// compute the relativistic correction term +double Glonass_GNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) +{ + double tk; + double a; + double n; + double n0; + double E; + double E_old; + double dE; + double M; + + // Restore semi-major axis + a = d_sqrt_A * d_sqrt_A; + + // Time from ephemeris reference epoch + tk = check_t(transmitTime - d_Toe); + + // Computed mean motion + n0 = sqrt(GM / (a * a * a)); + // Corrected mean motion + n = n0 + d_Delta_n; + // Mean anomaly + M = d_M_0 + n * tk; + + // Reduce mean anomaly to between 0 and 2pi + //M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); + + // Initial guess of eccentric anomaly + E = M; + + // --- Iteratively compute eccentric anomaly ---------------------------- + for (int ii = 1; ii < 20; ii++) + { + E_old = E; + E = M + d_e_eccentricity * sin(E); + dE = fmod(E - E_old, 2.0 * GPS_PI); + if (fabs(dE) < 1e-12) + { + //Necessary precision is reached, exit from the loop + break; + } + } + + // Compute relativistic correction term + d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E); + return d_dtr; +} + + +double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime) +{ + double dt = 0.0; + double tk = 0.0; + int numberOfIntegrations = 0; + + // Find time difference + dt = check_t(transmitTime - d_t_b); + + // Calculate clock correction + satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt); + + // Find integration time + tk = dt - satClkCorr; + + // Check if to integrate forward or backward + if tk < 0 + tau = -60; + else + tau = 60; + end + + // x,y,z coordinates to meters + x0 = d_satpos_X * 1e3; + y0 = d_satpos_Y * 1e3; + z0 = d_satpos_Z * 1e3; + + // x,y,z velocities to meters/s + Vx0 = d_satvel_X * 1e3; + Vy0 = d_satvel_Y * 1e3; + Vz0 = d_satvel_Z * 1e3; + + // x,y,z accelerations to meters/sec^2 + Ax0 = d_satacc_X * 1e3; + Ay0 = d_satacc_Y * 1e3; + Az0 = d_satacc_Z * 1e3; + + for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) + { + // Check if last integration step. If last integration step, make one more step that has the remaining time length... + if(fabs(numberOfIntegrations) > fabs(tk)) + { + // if there is more time left to integrate... + if mod(tk,tau) ~= 0 + { + tau = mod(time,tau); + } + else // otherwise make a zero step. + { + tau = 0; + } + } + + // Runge-Kutta Integration Stage K1 + r0 = sqrt(x0*x0 + y0*y0 + z0*z0); + r0_2 = r0*r0; + r0_3 = r0*r0*r0; + r0_5 = r0*r0*r0*r0*r0; + + dVx_1 = - GLONASS_GM / r0_3 * x0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * x0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * x0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy0 + Ax0; + dVy_1 = - GLONASS_GM / r0_3 * y0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * y0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * y0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx0 + Ay0; + dVz_1 = - GLONASS_GM / r0_3 * z0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * z0 * (3 - 5 * (z0*z0) / r0_2) + Az0; + + dx_1 = Vx0; + dy_1 = Vy0; + dz_1 = Vz0; + + // Runge-Kutta Integration Stage K2 + Vx_1 = Vx0 + 1/2 * tau * dVx_1; + Vy_1 = Vy0 + 1/2 * tau * dVy_1; + Vz_1 = Vz0 + 1/2 * tau * dVz_1; + + x_1 = x0 + 1/2 * tau * dx_1; + y_1 = y0 + 1/2 * tau * dy_1; + z_1 = z0 + 1/2 * tau * dz_1; + + r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); + r1_2 = r1*r1; + r1_3 = r1*r1*r1; + r1_5 = r1*r1*r1*r1*r1; + + dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0; + dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0; + dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0; + + dx_2 = Vx_1; + dy_2 = Vy_1; + dz_2 = Vz_1; + + // Runge-Kutta Integration Stage K2 + Vx_2 = Vx0 + 1/2 * tau * dVx_2; + Vy_2 = Vy0 + 1/2 * tau * dVy_2; + Vz_2 = Vz0 + 1/2 * tau * dVz_2; + + x_2 = x0 + 1/2 * tau * dx_2; + y_2 = y0 + 1/2 * tau * dy_2; + z_2 = z0 + 1/2 * tau * dz_2; + + r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); + r2_2 = r2*r2; + r2_3 = r2*r2*r2; + r2_5 = r2*r2*r2*r2*r2; + + + dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0; + dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0; + dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0; + + dx_3 = Vx_2; + dy_3 = Vy_2; + dz_3 = Vz_2; + + // Runge-Kutta Integration Stage K3 + Vx_3 = Vx0 + tau * dVx_3; + Vy_3 = Vy0 + tau * dVy_3; + Vz_3 = Vz0 + tau * dVz_3; + + x_3 = x0 + tau * dx_3; + y_3 = y0 + tau * dy_3; + z_3 = z0 + tau * dz_3; + + r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); + r3_2 = r3*r3; + r3_3 = r3*r3*r3; + r3_5 = r3*r3*r3*r3*r3; + + dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0; + dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0; + dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0; + + dx_4 = Vx_3; + dy_4 = Vy_3; + dz_4 = Vz_3; + + // Final results showcased here + Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); + Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); + Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); + + x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); + y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); + z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); + + } + + // Reasign position, velocities and accelerations for next integration + d_satpos_X = x0; + d_satpos_Y = y0; + d_satpos_Z = z0; + + d_satvel_X = Vx0; + d_satvel_Y = Vy0; + d_satvel_Z = Vz0; + + d_satacc_X = d_satacc_X; // No change in accelerations reported over interval + d_satacc_Y = d_satacc_Y; // No change in accelerations reported over interval + d_satacc_Z = d_satacc_Z; // No change in accelerations reported over interval + + // Time from ephemeris reference clock + tk = check_t(transmitTime - d_Toc); + + double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; + + /* relativity correction */ + dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); + + return dtr_s; +} + + +double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) +{ + double dt = 0.0; + double tk = 0.0; + int numberOfIntegrations = 0; + + // Find time difference + dt = check_t(transmitTime - d_t_b); + + // Calculate clock correction + satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt); + + // Find integration time + tk = dt - satClkCorr; + + // Check if to integrate forward or backward + if tk < 0 + tau = -60; + else + tau = 60; + end + + // Coordinates transformation to an inertial reference frame + // x,y,z coordinates to meters + x0 = d_satpos_X * 1e3; + y0 = d_satpos_Y * 1e3; + z0 = d_satpos_Z * 1e3; + + // x,y,z velocities to meters/s + Vx0 = d_satvel_X * 1e3; + Vy0 = d_satvel_Y * 1e3; + Vz0 = d_satvel_Z * 1e3; + + // x,y,z accelerations to meters/sec^2 + Ax0 = d_satacc_X * 1e3; + Ay0 = d_satacc_Y * 1e3; + Az0 = d_satacc_Z * 1e3; + + for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) + { + // Check if last integration step. If last integration step, make one more step that has the remaining time length... + if(fabs(numberOfIntegrations) > fabs(tk)) + { + // if there is more time left to integrate... + if mod(tk,tau) ~= 0 + { + tau = mod(time,tau); + } + else // otherwise make a zero step. + { + tau = 0; + } + } + + // Runge-Kutta Integration Stage K1 + r0 = sqrt(x0*x0 + y0*y0 + z0*z0); + r0_2 = r0*r0; + r0_3 = r0*r0*r0; + r0_5 = r0*r0*r0*r0*r0; + + dx_1 = Vx0; + dy_1 = Vy0; + dz_1 = Vz0; + + dVx_1 = - GLONASS_GM / r0_3 * x0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * x0 * (1 - 5 * 1 / (z0*z0)) + Jx_moon + Jx_sun; + dVy_1 = - GLONASS_GM / r0_3 * y0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * y0 * (1 - 5 * 1 / (z0*z0)) + Jy_moon + Jy_sun; + dVz_1 = - GLONASS_GM / r0_3 * z0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * z0 * (3 - 5 * 1 / (z0*z0)) + Jz_moon + Jz_sun; + + dx_1 = Vx0; + dy_1 = Vy0; + dz_1 = Vz0; + + // Runge-Kutta Integration Stage K2 + Vx_1 = Vx0 + 1/2 * tau * dVx_1; + Vy_1 = Vy0 + 1/2 * tau * dVy_1; + Vz_1 = Vz0 + 1/2 * tau * dVz_1; + + x_1 = x0 + 1/2 * tau * dx_1; + y_1 = y0 + 1/2 * tau * dy_1; + z_1 = z0 + 1/2 * tau * dz_1; + + r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); + r1_2 = r1*r1; + r1_3 = r1*r1*r1; + r1_5 = r1*r1*r1*r1*r1; + + dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0; + dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0; + dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0; + + dx_2 = Vx_1; + dy_2 = Vy_1; + dz_2 = Vz_1; + + // Runge-Kutta Integration Stage K2 + Vx_2 = Vx0 + 1/2 * tau * dVx_2; + Vy_2 = Vy0 + 1/2 * tau * dVy_2; + Vz_2 = Vz0 + 1/2 * tau * dVz_2; + + x_2 = x0 + 1/2 * tau * dx_2; + y_2 = y0 + 1/2 * tau * dy_2; + z_2 = z0 + 1/2 * tau * dz_2; + + r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); + r2_2 = r2*r2; + r2_3 = r2*r2*r2; + r2_5 = r2*r2*r2*r2*r2; + + + dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0; + dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0; + dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0; + + dx_3 = Vx_2; + dy_3 = Vy_2; + dz_3 = Vz_2; + + // Runge-Kutta Integration Stage K3 + Vx_3 = Vx0 + tau * dVx_3; + Vy_3 = Vy0 + tau * dVy_3; + Vz_3 = Vz0 + tau * dVz_3; + + x_3 = x0 + tau * dx_3; + y_3 = y0 + tau * dy_3; + z_3 = z0 + tau * dz_3; + + r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); + r3_2 = r3*r3; + r3_3 = r3*r3*r3; + r3_5 = r3*r3*r3*r3*r3; + + dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0; + dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0; + dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0; + + dx_4 = Vx_3; + dy_4 = Vy_3; + dz_4 = Vz_3; + + // Final results showcased here + Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); + Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); + Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); + + x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); + y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); + z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); + + } + + // Time from ephemeris reference clock + tk = check_t(transmitTime - d_Toc); + + double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; + + /* relativity correction */ + dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); + + return dtr_s; +} diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h new file mode 100644 index 000000000..6a2ab046d --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -0,0 +1,170 @@ +/*! + * \file glonass_gnav_ephemeris.h + * \brief Interface of a GLONASS EPHEMERIS storage + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_GNAV_EPHEMERIS_H_ +#define GNSS_SDR_GLONASS_GNAV_EPHEMERIS_H_ + + +#include +#include +#include "boost/assign.hpp" +#include + + + +/*! + * \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD Edition 5.1 + * + * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + */ +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); + + double gravitational_perturbations(const double mu); + + double d_Jx_moon; //!< Moon gravitational perturbation + double d_Jx_sun; //!< Sun gravitational perturbation + +public: + unsigned int i_satellite_freq_channel; // SV Frequency Channel Number + double d_m; //!< String number within frame [dimensionless] + double d_t_k; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] + double d_t_b; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] + double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] + double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] + double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + // satellite positions + double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + // Satellite acceleration + double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_B_n; //!< Health flag [dimensionless] + double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] + double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] + double d_F_T; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] + double d_n; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation + double d_Delta_tau_n; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] + double d_E_n; //!< Characterises "age" of a current information [days] + double d_P_1; //!< Flag of the immediate data updating. + double d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + double d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + + // clock terms derived from ephemeris data + double d_satClkDrift; //!< GPS clock error + double d_dtr; //!< relativistic clock correction term + + template + + /*! + * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. + */ + void serialize(Archive& archive, const unsigned int version) + { + using boost::serialization::make_nvp; + if(version){}; + + archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel); //!< SV PRN frequency channel number + archive & make_nvp("d_m", d_m); //!< String number within frame [dimensionless] + archive & make_nvp("d_t_k", d_t_k); //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] + archive & make_nvp("d_t_b", d_t_b); //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] + archive & make_nvp("d_M", d_M); //!< Type of satellite transmitting navigation signal [dimensionless] + archive & make_nvp("d_gamma_n", d_gamma_n); //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] + archive & make_nvp("d_tau_n", d_tau_n); //!< Correction to the nth satellite time (tn) relative to GLONASS time (te) + archive & make_nvp("d_B_n", d_B_n); //!< Health flag [dimensionless] + archive & make_nvp("d_P", d_P); //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] + archive & make_nvp("d_N_T", d_N_T); //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] + archive & make_nvp("d_F_T", d_F_T); //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] + archive & make_nvp("d_n", d_n); //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation + archive & make_nvp("d_Delta_tau_n", d_Delta_tau_n);//!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] + archive & make_nvp("d_E_n", d_E_n); //!< Characterises "age" of a current information [days] + archive & make_nvp("d_P_1", d_P_1); //!< Flag of the immediate data updating. + archive & make_nvp("d_P_2", d_P_2); //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + archive & make_nvp("d_P_3", d_P_3); //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + archive & make_nvp("d_P_4", d_P_4); //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + archive & make_nvp("d_l_n", d_l_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 Compute the ECEF SV coordinates and ECEF velocity + * Implementation of Algorithm A.3.1.2 in GLONASS ICD v5.1 + * and compute the clock bias term including relativistic effect (return value) + * \param transmitTime Time of ephemeris transmission + * \return clock bias of satellite + */ + double simplifiedSatellitePosition(double transmitTime); + + /*! + * \brief Compute the ECEF SV coordinates and ECEF velocity + * Implementation of Algorithm A.3.1.1 in GLONASS ICD v5.1 + * and compute the clock bias term including relativistic effect (return value) + * \param transmitTime Time of ephemeris transmission + * \return clock bias of satellite + */ + 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 + */ + Glonass_GNAV_Ephemeris(); +}; + +#endif diff --git a/src/core/system_parameters/glonass_gnav_utc_model.cc b/src/core/system_parameters/glonass_gnav_utc_model.cc new file mode 100644 index 000000000..921195298 --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_utc_model.cc @@ -0,0 +1,48 @@ +/* + * \file glonass_gnav_utc_model.h + * \brief Interface of a GLONASS GNAV UTC MODEL storage + * \author Damian Miralles, 2017. dmiralles2009(at).gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "glonass_gnav_utc_model.h" +#include + +Glonass_Gnav_Utc_Model::Glonass_Gnav_Utc_Model() +{ + valid = false; + d_tau_c = 0.0; +} + +double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected) +{ + double t_utc; + + // GLONASS Time is relative to UTC Moscow, so we simply add its time difference + t_utc = glonass_time_corrected + 3*3600 + d_tau_c; + + return t_utc; +} diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h new file mode 100644 index 000000000..b08c4bdce --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -0,0 +1,75 @@ +/*! + * \file glonass_gnav_utc_model.h + * \brief Interface of a GLONASS GNAV UTC MODEL storage + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_GNAV_UTC_MODEL_H_ +#define GNSS_SDR_GLONASS_GNAV_UTC_MODEL_H_ + +#include "boost/assign.hpp" +#include +#include + +/*! + * \brief This class is a storage for the GLONASS GNAV UTC MODEL data as described in GLONASS ICD (Edition 5.1) + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + */ +class Glonass_Gnav_Utc_Model +{ +public: + bool valid; + // UTC + double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] + + /*! + * 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); + + template + /* + * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. + */ + void serialize(Archive& archive, const unsigned int version) + { + using boost::serialization::make_nvp; + if(version){}; + archive & make_nvp("valid",valid); + archive & make_nvp("d_tau_c",d_tau_c); + } + +}; + +#endif From c24710c5855f6b032c5412b4c22e1c18a72d1cd9 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 20 Jun 2017 21:44:44 -0700 Subject: [PATCH 20/48] gnav: Adding GLONASS GNAV Navigation Message Operations Adding code for navigation message computation for GLONASS GNAV code. Continues to fix the almanac position computation and fixes bugs in code due to compilation errors, coding style and Doxygen documentation --- .gitignore | 1 + src/core/system_parameters/CMakeLists.txt | 5 +- src/core/system_parameters/GLONASS_L1_CA.h | 36 +- .../system_parameters/glonass_gnav_almanac.cc | 167 ++++- .../system_parameters/glonass_gnav_almanac.h | 65 +- .../glonass_gnav_ephemeris.cc | 638 +++++++++++------- .../glonass_gnav_ephemeris.h | 52 +- .../glonass_gnav_navigation_message.cc | 577 ++++++++++++++++ .../glonass_gnav_navigation_message.h | 203 ++++++ .../glonass_gnav_utc_model.cc | 7 +- .../glonass_gnav_utc_model.h | 37 +- src/tests/test_main.cc | 8 +- .../glonass_gnav_almanac_test.cc | 76 +++ .../glonass_gnav_ephemeris_test.cc | 54 ++ 14 files changed, 1604 insertions(+), 322 deletions(-) create mode 100644 src/core/system_parameters/glonass_gnav_navigation_message.cc create mode 100644 src/core/system_parameters/glonass_gnav_navigation_message.h create mode 100644 src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc create mode 100644 src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc diff --git a/.gitignore b/.gitignore index 2d1eebded..cbccacdac 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ docs/latex docs/GNSS-SDR_manual.pdf src/tests/data/output.dat thirdparty/ +.settings .project .cproject /install diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index df419aebd..9143a038b 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -39,6 +39,10 @@ set(SYSTEM_PARAMETERS_SOURCES gps_cnav_iono.cc gps_cnav_utc_model.cc rtcm.cc + glonass_gnav_ephemeris.cc + glonass_gnav_almanac.cc + glonass_gnav_utc_model.cc + glonass_gnav_navigation_message.cc ) @@ -59,4 +63,3 @@ add_library(gnss_system_parameters ${SYSTEM_PARAMETERS_SOURCES} ${SYSTEM_PARAMET source_group(Headers FILES ${SYSTEM_PARAMETERS_HEADERS}) add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE}) target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES}) - diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index d3c234801..cc5ff2db4 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -43,7 +43,7 @@ const double GLONASS_C_m_ms = 299792.4580; //!< The spe const double GLONASS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E const double GLONASS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200E const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s] -const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [km^3/s^2] +const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] const double GLONASS_fM_a = 0.35e9; //!< Gravitational constant of atmosphere [m^3/s^2] const double GLONASS_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m] const double GLONASS_FLATTENING = 1/29825784; //!< Flattening parameter @@ -56,26 +56,26 @@ const double GLONASS_J8 = 1.40e-11; //!< Eighth const double GLONASS_U0 = 62636861.4; //!< Normal potential at surface of common terrestrial ellipsoid [m^2/s^2] const double GLONASS_C20 = -1082.63e-6; //!< Second zonal coefficient of spherical harmonic expansion const double GLONASS_EARTH_RADIUS = 6378.136; //!< Equatorial radius of Earth [km] -const double GLONASS_EARTH_INCLINATION = 23°26'33'' //!< Mean inclination of ecliptic to equator (23°26'33''). +const double GLONASS_EARTH_INCLINATION = 0.000409148809899e3; //!< Mean inclination of ecliptic to equator (23 deg 26 min 33 sec) [rad] -const double GLONASS_TAU_0 = -334°19′46′′,40; -const double GLONASS_TAU_1 = 4069°02′02′′,52; +const double GLONASS_TAU_0 = -0.005835151531174e3; //!< (-334 deg 19 min 46.40 sec) [rad]; +const double GLONASS_TAU_1 = 0.071018041257371e3; //!< (4069 deg 02 min 02.52 sec) [rad]; -const double GLONASS_MOON_Q0 = -63°53′43′′,41; -const double GLONASS_MOON_Q1 = 477198°50′56′′,79; -const double GLONASS_MOON_OMEGA_0 = 259°10′59′′,79; -const double GLONASS_MOON_OMEGA_1 = -1934°08′31′′,23; +const double GLONASS_MOON_Q0 = -0.001115184961435e3; //!< (-63 deg 53 min 43.41 sec) [rad] +const double GLONASS_MOON_Q1 = 8.328691103668023e3; //!< (477198 deg 50 min 56.79 sec) [rad] +const double GLONASS_MOON_OMEGA_0 = 0.004523601514852e3; //!< (259 deg 10 min 59.79 sec) [rad] +const double GLONASS_MOON_OMEGA_1 = -0.033757146246552e3; //!< (-1934 deg 08 min 31.23 sec) [rad] const double GLONASS_MOON_GM = 4902.835; //!< Lunar gravitational constant [km^3/s^2] const double GLONASS_MOON_SEMI_MAJOR_AXIS = 3.84385243e5; //!< Semi-major axis of lunar orbit [km]; const double GLONASS_MOON_ECCENTRICITY = 0.054900489; //!< Eccentricity of lunar orbit -const double GLONASS_MOON_INCLINATION = 5°08'43.4'' //!< Inclination of lunar orbit to ecliptic plane (5°08'43.4'') +const double GLONASS_MOON_INCLINATION = 0.000089803977407e3; //!< Inclination of lunar orbit to ecliptic plane (5 deg 08 min 43.4 sec) [rad] -const double GLONASS_SUN_OMEGA = 281°13′15′′,0 + 6189′′, 03Т; -const double GLONASS_SUN_Q0 = 358°28′33′′,04; -const double GLONASS_SUN_Q1 = 129596579′′,10; -const double GLONASS_SUN_GM = 0.1325263e12; //!< Solar gravitational constant [km^3/s^2] -const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km]; -const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit +const double GLONASS_SUN_OMEGA = 0.004908229466869e3; //!< TODO What is this operation in the seconds with T?(281 deg 13 min 15.0 + 6189.03*Т sec) [rad] +const double GLONASS_SUN_Q0 = 0.006256583774423e3; //!< (358 deg 28 min 33.04 sec) [rad] +const double GLONASS_SUN_Q1 = 0e3; //!< TODO Why is the value greater than 60?(129596579.10 sec) [rad] +const double GLONASS_SUN_GM = 0.1325263e12; //!< Solar gravitational constant [km^3/s^2] +const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km]; +const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit // carrier and code frequencies const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] @@ -133,7 +133,7 @@ const std::vector> Y_N({{51,27}}); const std::vector> P3({{6,1}}); const std::vector> GAMMA_N({{7,11}}); const std::vector> P({{19,2}}); -const std::vector> L_N({{21,1}}); +const std::vector> EPH_L_N({{21,1}}); const std::vector> Z_N_DOT ({{22,24}}); const std::vector> Z_N_DOT_DOT ({{46,5}}); const std::vector> Z_N({{51,27}}); @@ -153,7 +153,7 @@ const std::vector> N_A({{6,11}}); const std::vector> TAU_C({{17,32}}); const std::vector> N_4({{50,5}}); const std::vector> TAU_GPS({{55,22}}); -const std::vector> L_N({{77,1}}); +const std::vector> ALM_L_N({{77,1}}); // STRING 6, 8, 10, 12, 14 const std::vector> C_N({{6,1}}); @@ -170,7 +170,7 @@ const std::vector> T_LAMBDA_N_A({{22,21}}); const std::vector> DELTA_T_N_A({{43,22}}); const std::vector> DELTA_T_DOT_N_A({{65,7}}); const std::vector> H_N_A({{72,5}}); -const std::vector> L_N({{77,1}}); +//const std::vector> L_N({{77,1}}); #endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index 45174cabb..ae1f492c7 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -1,8 +1,7 @@ /*! * \file glonass_gnav_almanac.cc - * \brief Interface of a GLONASS GNAV ALMANAC storage - * - * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * \brief Interface of a GLONASS GNAV ALMANAC storage as described in GLONASS ICD (Edition 5.1) + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf * \author Damian Miralles , 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- @@ -29,29 +28,149 @@ * * ------------------------------------------------------------------------- */ - #include "glonass_gnav_almanac.h" +#include +#include "GLONASS_L1_CA.h" +#include "gnss_satellite.h" Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() { - int i_satellite_freq_channel = 0; - double d_tau_c = 0.0; - double d_tau_gps = 0.0; - double d_N_4 = 0.0; - double d_N_A = 0.0; - double d_n_A = 0.0; - double d_H_n_A = 0.0; - double d_lambda_n_A = 0.0; - double d_t_lambda_n_A = 0.0; - double d_Delta_i_n_A = 0.0; - double d_Delta_T_n_A = 0.0; - double d_Delta_T_n_A_dot = 0.0; - double d_epsilon_n_A = 0.0; - double d_omega_n_A = 0.0; - double d_M_n_A = 0.0; - double d_B1 = 0.0; - double d_B2 = 0.0; - double d_KP = 0.0; - double d_tau_n_A = 0.0; - double d_C_n_A = 0.0; + i_satellite_freq_channel = 0; + d_tau_c = 0.0; + d_tau_gps = 0.0; + d_N_4 = 0.0; + d_N_A = 0.0; + d_n_A = 0.0; + d_H_n_A = 0.0; + d_lambda_n_A = 0.0; + d_t_lambda_n_A = 0.0; + d_Delta_i_n_A = 0.0; + d_Delta_T_n_A = 0.0; + d_Delta_T_n_A_dot = 0.0; + d_epsilon_n_A = 0.0; + d_omega_n_A = 0.0; + d_M_n_A = 0.0; + d_KP = 0.0; + d_tau_n_A = 0.0; + d_C_n_A = 0.0; +} + +void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) +{ + double T_nom = 43200; // [seconds] + double i_nom = D2R*63.0; // [rad] + + double Delta_t = 0.0; + double i = 0.0; + double T = 0.0; + double n = 0.0; + double a = 0.0; + double lambda_dot = 0.0; + double omega_dot = 0.0; + double lambda = 0.0; + double omega = 0.0; + double E_P = 0.0; + double Delta_T = 0.0; + double M = 0.0; + double E = 0.0; + double E_old = 0.0; + double dE = 0.0; + + double e1_x = 0.0; + double e1_y = 0.0; + double e1_z = 0.0; + + double e2_x = 0.0; + double e2_y = 0.0; + double e2_z = 0.0; + // Compute time difference to reference time + Delta_t = (N_i - d_N_A) * 86400 + (t_i + d_t_lambda_n_A); + + // Compute the actual inclination + i = i_nom + d_Delta_i_n_A; + + // Compute the actual orbital period: + T = T_nom + d_Delta_T_n_A; + + // Compute the mean motion + n = 2*GLONASS_PI/T; + + // Compute the semi-major axis: + a = cbrt(GLONASS_GM/(n*n)); + + // Compute correction to longitude of ascending node + lambda_dot = -10*pow(GLONASS_SEMI_MAJOR_AXIS / a, 7/2)*D2R*cos(i)/86400; + + // Compute correction to argument of perigee + omega_dot = 5*pow(GLONASS_SEMI_MAJOR_AXIS / a, 7/2)*D2R*(5*cos(i)*cos(i) - 1)/86400; + + // Compute corrected longitude of ascending node: + lambda = d_lambda_n_A + (lambda_dot - GLONASS_OMEGA_EARTH_DOT)*Delta_t; + + // Compute corrected argument of perigee: + omega = d_omega_n_A + omega_dot*Delta_t; + + // Compute eccentric anomaly at point P: Note: P is that point of the orbit the true anomaly of which is identical to the argument of perigee. + E_P = 2*atan(tan((omega/2)*(sqrt((1 - d_epsilon_n_A)*(1 + d_epsilon_n_A))))); + + // Compute time difference to perigee passing + if (omega < GLONASS_PI) + { + Delta_T = (E_P - d_epsilon_n_A*sin(E_P))/n; + } + else + { + Delta_T = (E_P - d_epsilon_n_A*sin(E_P))/n + T; + } + + // Compute mean anomaly at epoch t_i: + M = n * (Delta_t - Delta_T); + + // Compute eccentric anomaly at epoch t_i. Note: Kepler’s equation has to be solved iteratively + + // Initial guess of eccentric anomaly + E = M; + + // --- Iteratively compute eccentric anomaly ---------------------------- + for (int ii = 1; ii < 20; ii++) + { + E_old = E; + E = M + d_epsilon_n_A * sin(E); + dE = fmod(E - E_old, 2.0 * GLONASS_PI); + if (fabs(dE) < 1e-12) + { + //Necessary precision is reached, exit from the loop + break; + } + } + + // Compute position in orbital coordinate system + d_satpos_Xo = a*cos(E) - d_epsilon_n_A; + d_satpos_Yo = a*sqrt(1 - d_epsilon_n_A*d_epsilon_n_A)*sin(E); + d_satpos_Zo = a*0; + + // Compute velocity in orbital coordinate system + d_satvel_Xo = a/(1-d_epsilon_n_A*cos(E))*(-n*sin(E)); + d_satvel_Yo = a/(1-d_epsilon_n_A*cos(E))*(n*sqrt(1 - d_epsilon_n_A*d_epsilon_n_A)*cos(E)); + d_satvel_Zo = a/(1-d_epsilon_n_A*cos(E))*(0); + + // Determine orientation vectors of orbital coordinate system in ECEF system + e1_x = cos(omega)*cos(lambda) - sin(omega)*sin(lambda); + e1_y = cos(omega)*sin(lambda) + sin(omega)*cos(lambda)*cos(i); + e1_z = sin(omega)*sin(i); + + e2_x = -sin(omega)*cos(lambda) - sin(omega)*sin(lambda)*cos(i); + e2_y = -sin(omega)*sin(lambda) + cos(omega)*cos(lambda)*cos(i); + e2_z = cos(omega)*sin(i); + + // Convert position from orbital to ECEF system + d_satpos_X = d_satpos_Xo*e1_x + d_satpos_Xo*e2_x; + d_satpos_Y = d_satpos_Yo*e1_z + d_satpos_Yo*e2_y; + d_satpos_Z = d_satpos_Zo*e1_z + d_satpos_Zo*e2_z; + + // Convert position from orbital to ECEF system + d_satvel_X = d_satvel_Xo*e1_x + d_satvel_Xo*e2_x + GLONASS_OMEGA_EARTH_DOT*d_satpos_Y; + d_satvel_Y = d_satvel_Yo*e1_z + d_satvel_Yo*e2_y - GLONASS_OMEGA_EARTH_DOT*d_satpos_X; + d_satvel_Z = d_satvel_Zo*e1_z + d_satvel_Zo*e2_z; + } diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 3e13802fc..f4ce97312 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -32,16 +32,20 @@ #ifndef GNSS_SDR_GLONASS_ALMANAC_H_ #define GNSS_SDR_GLONASS_ALMANAC_H_ +#include +#include +#include "boost/assign.hpp" +#include /*! - * \brief This class is a storage for the GLONASS SV ALMANAC data as described in IS-GPS-200E - * \todo Add proper links for GLONASS ICD - * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * \brief This class is a storage for the GLONASS SV ALMANAC data as described GLONASS ICD (Edition 5.1) + * + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf */ class Glonass_Gnav_Almanac { public: - int i_satellite_freq_channel; //!< SV PRN NUMBER + int i_satellite_freq_channel; //!< SV Frequency Channel NUMBER double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] @@ -50,17 +54,64 @@ public: double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] double d_t_lambda_n_A; //!< Time of first ascending node passage [s] - double d_Delta_i_n_A //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] + double d_Delta_i_n_A; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] double d_Delta_T_n_A; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period] double d_Delta_T_n_A_dot; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2] double d_epsilon_n_A; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless] double d_omega_n_A; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles] double d_M_n_A; //!< Type of satellite n_A [dimensionless] - double d_B1; //!< Coefficient to determine DeltaUT1 [s] - double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd] double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless] double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] double d_C_n_A; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + + // satellite positions + double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_satpos_Yo; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_satpos_Zo; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + double d_satvel_Xo; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Yo; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Zo; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + + // satellite positions + double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + + 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. + */ + void serialize(Archive& archive, const unsigned int version) + { + using boost::serialization::make_nvp; + if(version){}; + + archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel); + archive & make_nvp("d_tau_c", d_tau_c); + archive & make_nvp("d_tau_gps", d_tau_gps); + archive & make_nvp("d_N_4", d_N_4); + archive & make_nvp("d_N_A", d_N_A); + archive & make_nvp("d_n_A", d_n_A); + archive & make_nvp("d_H_n_A", d_H_n_A); + archive & make_nvp("d_lambda_n_A", d_lambda_n_A); + archive & make_nvp("d_t_lambda_n_A", d_t_lambda_n_A); + archive & make_nvp("d_Delta_i_n_A", d_Delta_i_n_A); + archive & make_nvp("d_Delta_T_n_A", d_Delta_T_n_A); + archive & make_nvp("d_Delta_T_n_A_dot", d_Delta_T_n_A_dot); + archive & make_nvp("d_epsilon_n_A", d_epsilon_n_A); + archive & make_nvp("d_omega_n_A", d_omega_n_A); + archive & make_nvp("d_M_n_A", d_M_n_A); + archive & make_nvp("d_KP", d_KP); + archive & make_nvp("d_tau_n_A", d_tau_n_A); + archive & make_nvp("d_C_n_A", d_C_n_A); + } + + void satellite_position(double N_i, double t_i); /*! * Default constructor */ diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index a6e6436c2..7f48c1051 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -1,9 +1,11 @@ /*! - * \file gps_ephemeris.cc - * \brief Interface of a GPS EPHEMERIS storage and orbital model functions + * \file glonass_gnav_ephemeris.cc + * \brief Interface of a GLONASS GNAV EPHEMERIS storage and orbital model functions * - * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II - * \author Javier Arribas, 2013. jarribas(at)cttc.es + * See Russian Institute of Space Device Engineering, + “Global Navigation Satellite System GLONASS Interface Control Document, + Navigational radiosignal in bands L1, L2,” Moscow, Edition 5.1, 2008. + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- * @@ -30,12 +32,12 @@ * ------------------------------------------------------------------------- */ -#include "gps_ephemeris.h" +#include "glonass_gnav_ephemeris.h" #include #include "GLONASS_L1_CA.h" #include "gnss_satellite.h" -Gps_Ephemeris::Gps_Ephemeris() +Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() { i_satellite_freq_channel = 0; d_m = 0.0; //!< String number within frame [dimensionless] @@ -75,53 +77,56 @@ Gps_Ephemeris::Gps_Ephemeris() } -void Glonass_GNAV_Ephemeris::gravitational_perturbations() +void Glonass_Gnav_Ephemeris::gravitational_perturbations() { - double T; - double sigma_days; - double tau_prime; - double Omega_moon; - double q_moon; - double q_sun; - double Tau_11; double Tau_12; - double Eta_11; double Eta_12; - double xi_11; double xi_12; - double xi_star; - double eta_star; - double zeta_star; - double E_moon; - double E_sun; - double xi_11; - double xi_12; - double eta_11; - double eta_12; - double zeta_11; - double zeta_12; + double T = 0.0; + double sigma_days = 0.0; + double tau_prime = 0.0; + double Omega_moon = 0.0; + double q_moon = 0.0; + double q_sun = 0.0; - double sin_theta_moon; - double cos_theta_moon; - double theta_moon; - double xi_moon_e; - double eta_moon_e; - double zeta_moon_e; + double xi_star = 0.0; + double eta_star = 0.0; + double zeta_star = 0.0; + double E_moon = 0.0; + double E_sun = 0.0; + double xi_11 = 0.0; + double xi_12 = 0.0; + double eta_11 = 0.0; + double eta_12 = 0.0; + double zeta_11 = 0.0; + double zeta_12 = 0.0; - double sin_theta_sun; - double cos_theta_sun; - double theta_sun; - double xi_sun_e; - double eta_sun_e; - double zeta_sun_e; + double sin_theta_moon = 0.0; + double cos_theta_moon = 0.0; + double theta_moon = 0.0; + double xi_moon_e = 0.0; + double eta_moon_e = 0.0; + double zeta_moon_e = 0.0; + double r_moon_e = 0.0; - double r_moon_e; - double mu_bar_moon; - double x_bar_moon; - double y_bar_moon; - double Delta_o_moon; - double mu_bar_sun; - double x_bar_sun; - double y_bar_sun; - double Delta_o_sun; + double sin_theta_sun = 0.0; + double cos_theta_sun = 0.0; + double theta_sun = 0.0; + double xi_sun_e = 0.0; + double eta_sun_e = 0.0; + double zeta_sun_e = 0.0; + double r_sun_e = 0.0; + double mu_bar_moon = 0.0; + double x_bar_moon = 0.0; + double y_bar_moon = 0.0; + double z_bar_moon = 0.0; + double Delta_o_moon = 0.0; + double mu_bar_sun = 0.0; + double x_bar_sun = 0.0; + double y_bar_sun = 0.0; + double z_bar_sun = 0.0; + double Delta_o_sun = 0.0; + + + double t_e = 0.0; // fabs(tk)) { // if there is more time left to integrate... - if mod(tk,tau) ~= 0 + if(fmod(tk,tau) != 0) { - tau = mod(time,tau); + tau = fmod(tk,tau); } else // otherwise make a zero step. { @@ -322,49 +382,49 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime) } // Runge-Kutta Integration Stage K1 - r0 = sqrt(x0*x0 + y0*y0 + z0*z0); + r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0); r0_2 = r0*r0; r0_3 = r0*r0*r0; r0_5 = r0*r0*r0*r0*r0; - dVx_1 = - GLONASS_GM / r0_3 * x0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * x0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * x0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy0 + Ax0; - dVy_1 = - GLONASS_GM / r0_3 * y0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * y0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * y0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx0 + Ay0; - dVz_1 = - GLONASS_GM / r0_3 * z0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * z0 * (3 - 5 * (z0*z0) / r0_2) + Az0; + dVx_1 = - GLONASS_GM / r0_3 * x_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * x_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_0 + Ax_0; + dVy_1 = - GLONASS_GM / r0_3 * y_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * y_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_0 + Ay_0; + dVz_1 = - GLONASS_GM / r0_3 * z_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * z_0 * (3 - 5 * (z_0*z_0) / r0_2) + Az_0; - dx_1 = Vx0; - dy_1 = Vy0; - dz_1 = Vz0; + dx_1 = Vx_0; + dy_1 = Vy_0; + dz_1 = Vz_0; // Runge-Kutta Integration Stage K2 - Vx_1 = Vx0 + 1/2 * tau * dVx_1; - Vy_1 = Vy0 + 1/2 * tau * dVy_1; - Vz_1 = Vz0 + 1/2 * tau * dVz_1; + Vx_1 = Vx_0 + 1/2 * tau * dVx_1; + Vy_1 = Vy_0 + 1/2 * tau * dVy_1; + Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - x_1 = x0 + 1/2 * tau * dx_1; - y_1 = y0 + 1/2 * tau * dy_1; - z_1 = z0 + 1/2 * tau * dz_1; + x_1 = x_0 + 1/2 * tau * dx_1; + y_1 = y_0 + 1/2 * tau * dy_1; + z_1 = z_0 + 1/2 * tau * dz_1; r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); r1_2 = r1*r1; r1_3 = r1*r1*r1; r1_5 = r1*r1*r1*r1*r1; - dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0; - dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0; - dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0; + dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0; + dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0; + dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; dx_2 = Vx_1; dy_2 = Vy_1; dz_2 = Vz_1; // Runge-Kutta Integration Stage K2 - Vx_2 = Vx0 + 1/2 * tau * dVx_2; - Vy_2 = Vy0 + 1/2 * tau * dVy_2; - Vz_2 = Vz0 + 1/2 * tau * dVz_2; + Vx_2 = Vx_0 + 1/2 * tau * dVx_2; + Vy_2 = Vy_0 + 1/2 * tau * dVy_2; + Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - x_2 = x0 + 1/2 * tau * dx_2; - y_2 = y0 + 1/2 * tau * dy_2; - z_2 = z0 + 1/2 * tau * dz_2; + x_2 = x_0 + 1/2 * tau * dx_2; + y_2 = y_0 + 1/2 * tau * dy_2; + z_2 = z_0 + 1/2 * tau * dz_2; r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); r2_2 = r2*r2; @@ -372,109 +432,205 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime) r2_5 = r2*r2*r2*r2*r2; - dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0; - dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0; - dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0; + dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0; + dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0; + dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; dx_3 = Vx_2; dy_3 = Vy_2; dz_3 = Vz_2; // Runge-Kutta Integration Stage K3 - Vx_3 = Vx0 + tau * dVx_3; - Vy_3 = Vy0 + tau * dVy_3; - Vz_3 = Vz0 + tau * dVz_3; + Vx_3 = Vx_0 + tau * dVx_3; + Vy_3 = Vy_0 + tau * dVy_3; + Vz_3 = Vz_0 + tau * dVz_3; - x_3 = x0 + tau * dx_3; - y_3 = y0 + tau * dy_3; - z_3 = z0 + tau * dz_3; + x_3 = x_0 + tau * dx_3; + y_3 = y_0 + tau * dy_3; + z_3 = z_0 + tau * dz_3; r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); r3_2 = r3*r3; r3_3 = r3*r3*r3; r3_5 = r3*r3*r3*r3*r3; - dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0; - dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0; - dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0; + dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0; + dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0; + dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; dx_4 = Vx_3; dy_4 = Vy_3; dz_4 = Vz_3; // Final results showcased here - Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); - Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); - Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); + Vx_0 = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); + Vy_0 = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); + Vz_0 = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); - x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); - y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); - z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); + x_0 = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); + y_0 = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); + z_0 = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); } // Reasign position, velocities and accelerations for next integration - d_satpos_X = x0; - d_satpos_Y = y0; - d_satpos_Z = z0; + d_satpos_X = x_0; + d_satpos_Y = y_0; + d_satpos_Z = z_0; - d_satvel_X = Vx0; - d_satvel_Y = Vy0; - d_satvel_Z = Vz0; + d_satvel_X = Vx_0; + d_satvel_Y = Vy_0; + d_satvel_Z = Vz_0; - d_satacc_X = d_satacc_X; // No change in accelerations reported over interval - d_satacc_Y = d_satacc_Y; // No change in accelerations reported over interval - d_satacc_Z = d_satacc_Z; // No change in accelerations reported over interval + d_satacc_X = d_AXn; // No change in accelerations reported over interval + d_satacc_Y = d_AYn; // No change in accelerations reported over interval + d_satacc_Z = d_AZn; // No change in accelerations reported over interval // Time from ephemeris reference clock - tk = check_t(transmitTime - d_Toc); + //tk = check_t(transmitTime - d_Toc); - double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; + //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; /* relativity correction */ - dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); + //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); - return dtr_s; + return 0; } -double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) +double Glonass_Gnav_Ephemeris::satellite_position(double transmitTime) { double dt = 0.0; double tk = 0.0; + double tau = 0.0; int numberOfIntegrations = 0; + // RK 1 + double x_0 = 0.0; + double y_0 = 0.0; + double z_0 = 0.0; + + double Vx_0 = 0.0; + double Vy_0 = 0.0; + double Vz_0 = 0.0; + + double Ax_0 = 0.0; + double Ay_0 = 0.0; + double Az_0 = 0.0; + + double r0 = 0.0; + double r0_2 = 0.0; + double r0_3 = 0.0; + double r0_5 = 0.0; + + double dVx_1 = 0.0; + double dVy_1 = 0.0; + double dVz_1 = 0.0; + + double dx_1 = 0.0; + double dy_1 = 0.0; + double dz_1 = 0.0; + + // Runge-Kutta Integration Stage K2 + double x_1 = 0.0; + double y_1 = 0.0; + double z_1 = 0.0; + + double Vx_1 = 0.0; + double Vy_1 = 0.0; + double Vz_1 = 0.0; + + double r1 = 0.0; + double r1_2 = 0.0; + double r1_3 = 0.0; + double r1_5 = 0.0; + + double dVx_2 = 0.0; + double dVy_2 = 0.0; + double dVz_2 = 0.0; + + double dx_2 = 0.0; + double dy_2 = 0.0; + double dz_2 = 0.0; + + // Runge-Kutta Integration Stage K3 + double x_2 = 0.0; + double y_2 = 0.0; + double z_2 = 0.0; + + double Vx_2 = 0.0; + double Vy_2 = 0.0; + double Vz_2 = 0.0; + + double r2 = 0.0; + double r2_2 = 0.0; + double r2_3 = 0.0; + double r2_5 = 0.0; + + double dVx_3 = 0.0; + double dVy_3 = 0.0; + double dVz_3 = 0.0; + + double dx_3 = 0.0; + double dy_3 = 0.0; + double dz_3 = 0.0; + + // Runge-Kutta Integration Stage K4 + double x_3 = 0.0; + double y_3 = 0.0; + double z_3 = 0.0; + + double Vx_3 = 0.0; + double Vy_3 = 0.0; + double Vz_3 = 0.0; + + double r3 = 0.0; + double r3_2 = 0.0; + double r3_3 = 0.0; + double r3_5 = 0.0; + + double dVx_4 = 0.0; + double dVy_4 = 0.0; + double dVz_4 = 0.0; + + double dx_4 = 0.0; + double dy_4 = 0.0; + double dz_4 = 0.0; + // Find time difference dt = check_t(transmitTime - d_t_b); // Calculate clock correction - satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt); + d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt); // Find integration time - tk = dt - satClkCorr; + tk = dt - d_satClkDrift; // Check if to integrate forward or backward - if tk < 0 - tau = -60; + if (tk < 0) + { + tau = -60; + } else - tau = 60; - end + { + tau = 60; + } // Coordinates transformation to an inertial reference frame // x,y,z coordinates to meters - x0 = d_satpos_X * 1e3; - y0 = d_satpos_Y * 1e3; - z0 = d_satpos_Z * 1e3; + x_0 = d_Xn * 1e3; + y_0 = d_Yn * 1e3; + z_0 = d_Zn * 1e3; // x,y,z velocities to meters/s - Vx0 = d_satvel_X * 1e3; - Vy0 = d_satvel_Y * 1e3; - Vz0 = d_satvel_Z * 1e3; + Vx_0 = d_VXn * 1e3; + Vy_0 = d_VYn * 1e3; + Vz_0 = d_VZn * 1e3; // x,y,z accelerations to meters/sec^2 - Ax0 = d_satacc_X * 1e3; - Ay0 = d_satacc_Y * 1e3; - Az0 = d_satacc_Z * 1e3; + Ax_0 = d_AXn * 1e3; + Ay_0 = d_AYn * 1e3; + Az_0 = d_AZn * 1e3; for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) { @@ -482,9 +638,9 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) if(fabs(numberOfIntegrations) > fabs(tk)) { // if there is more time left to integrate... - if mod(tk,tau) ~= 0 + if (fmod(tk,tau) != 0) { - tau = mod(time,tau); + tau = fmod(tk,tau); } else // otherwise make a zero step. { @@ -493,53 +649,53 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) } // Runge-Kutta Integration Stage K1 - r0 = sqrt(x0*x0 + y0*y0 + z0*z0); + r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0); r0_2 = r0*r0; r0_3 = r0*r0*r0; r0_5 = r0*r0*r0*r0*r0; - dx_1 = Vx0; - dy_1 = Vy0; - dz_1 = Vz0; + dx_1 = Vx_0; + dy_1 = Vy_0; + dz_1 = Vz_0; - dVx_1 = - GLONASS_GM / r0_3 * x0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * x0 * (1 - 5 * 1 / (z0*z0)) + Jx_moon + Jx_sun; - dVy_1 = - GLONASS_GM / r0_3 * y0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * y0 * (1 - 5 * 1 / (z0*z0)) + Jy_moon + Jy_sun; - dVz_1 = - GLONASS_GM / r0_3 * z0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * z0 * (3 - 5 * 1 / (z0*z0)) + Jz_moon + Jz_sun; + dVx_1 = - GLONASS_GM / r0_3 * x_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * x_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jx_moon + d_Jx_sun; + dVy_1 = - GLONASS_GM / r0_3 * y_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * y_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jy_moon + d_Jy_sun; + dVz_1 = - GLONASS_GM / r0_3 * z_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * z_0 * (3 - 5 * 1 / (z_0*z_0)) + d_Jz_moon + d_Jz_sun; - dx_1 = Vx0; - dy_1 = Vy0; - dz_1 = Vz0; + dx_1 = Vx_0; + dy_1 = Vy_0; + dz_1 = Vz_0; // Runge-Kutta Integration Stage K2 - Vx_1 = Vx0 + 1/2 * tau * dVx_1; - Vy_1 = Vy0 + 1/2 * tau * dVy_1; - Vz_1 = Vz0 + 1/2 * tau * dVz_1; + Vx_1 = Vx_0 + 1/2 * tau * dVx_1; + Vy_1 = Vy_0 + 1/2 * tau * dVy_1; + Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - x_1 = x0 + 1/2 * tau * dx_1; - y_1 = y0 + 1/2 * tau * dy_1; - z_1 = z0 + 1/2 * tau * dz_1; + x_1 = x_0 + 1/2 * tau * dx_1; + y_1 = y_0 + 1/2 * tau * dy_1; + z_1 = z_0 + 1/2 * tau * dz_1; r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); r1_2 = r1*r1; r1_3 = r1*r1*r1; r1_5 = r1*r1*r1*r1*r1; - dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0; - dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0; - dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0; + dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0; + dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0; + dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; dx_2 = Vx_1; dy_2 = Vy_1; dz_2 = Vz_1; // Runge-Kutta Integration Stage K2 - Vx_2 = Vx0 + 1/2 * tau * dVx_2; - Vy_2 = Vy0 + 1/2 * tau * dVy_2; - Vz_2 = Vz0 + 1/2 * tau * dVz_2; + Vx_2 = Vx_0 + 1/2 * tau * dVx_2; + Vy_2 = Vy_0 + 1/2 * tau * dVy_2; + Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - x_2 = x0 + 1/2 * tau * dx_2; - y_2 = y0 + 1/2 * tau * dy_2; - z_2 = z0 + 1/2 * tau * dz_2; + x_2 = x_0 + 1/2 * tau * dx_2; + y_2 = y_0 + 1/2 * tau * dy_2; + z_2 = z_0 + 1/2 * tau * dz_2; r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); r2_2 = r2*r2; @@ -547,54 +703,54 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) r2_5 = r2*r2*r2*r2*r2; - dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0; - dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0; - dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0; + dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0; + dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0; + dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; dx_3 = Vx_2; dy_3 = Vy_2; dz_3 = Vz_2; // Runge-Kutta Integration Stage K3 - Vx_3 = Vx0 + tau * dVx_3; - Vy_3 = Vy0 + tau * dVy_3; - Vz_3 = Vz0 + tau * dVz_3; + Vx_3 = Vx_0 + tau * dVx_3; + Vy_3 = Vy_0 + tau * dVy_3; + Vz_3 = Vz_0 + tau * dVz_3; - x_3 = x0 + tau * dx_3; - y_3 = y0 + tau * dy_3; - z_3 = z0 + tau * dz_3; + x_3 = x_0 + tau * dx_3; + y_3 = y_0 + tau * dy_3; + z_3 = z_0 + tau * dz_3; r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); r3_2 = r3*r3; r3_3 = r3*r3*r3; r3_5 = r3*r3*r3*r3*r3; - dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0; - dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0; - dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0; + dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0; + dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0; + dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; dx_4 = Vx_3; dy_4 = Vy_3; dz_4 = Vz_3; // Final results showcased here - Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); - Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); - Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); + d_satvel_X = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); + d_satvel_Y = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); + d_satvel_Z = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); - x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); - y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); - z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); + d_satpos_X = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); + d_satpos_Y = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); + d_satpos_Z = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); } // Time from ephemeris reference clock - tk = check_t(transmitTime - d_Toc); + //tk = check_t(transmitTime - d_Toc); - double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; + //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; /* relativity correction */ - dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); + //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); - return dtr_s; + return d_dtr; } diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 6a2ab046d..d0edcc69b 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -41,11 +41,11 @@ /*! - * \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD Edition 5.1 + * \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD (Edition 5.1) * - * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf */ -class Glonass_GNAV_Ephemeris +class Glonass_Gnav_Ephemeris { private: /* @@ -57,10 +57,15 @@ private: */ double check_t(double time); - double gravitational_perturbations(const double mu); + void gravitational_perturbations(); double d_Jx_moon; //!< Moon gravitational perturbation + double d_Jy_moon; //!< Moon gravitational perturbation + double d_Jz_moon; //!< Moon gravitational perturbation + double d_Jx_sun; //!< Sun gravitational perturbation + double d_Jy_sun; //!< Sun gravitational perturbation + double d_Jz_sun; //!< Sun gravitational perturbation public: unsigned int i_satellite_freq_channel; // SV Frequency Channel Number @@ -70,18 +75,20 @@ public: double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + // satellite positions - double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + double d_Xn; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_Yn; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_Zn; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] // Satellite velocity - double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + double d_VXn; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_VYn; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_VZn; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] // Satellite acceleration - double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_AXn; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_AYn; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_AZn; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_B_n; //!< Health flag [dimensionless] double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] @@ -99,6 +106,19 @@ public: double d_satClkDrift; //!< GPS clock error double d_dtr; //!< relativistic clock correction term + // satellite positions + double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + // Satellite acceleration + double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + template /*! @@ -137,7 +157,7 @@ public: * \param transmitTime Time of ephemeris transmission * \return clock bias of satellite */ - double simplifiedSatellitePosition(double transmitTime); + double simplified_satellite_position(double transmitTime); /*! * \brief Compute the ECEF SV coordinates and ECEF velocity @@ -146,7 +166,7 @@ public: * \param transmitTime Time of ephemeris transmission * \return clock bias of satellite */ - double satellitePosition(double transmitTime); + double satellite_position(double transmitTime); /*! * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction @@ -164,7 +184,7 @@ public: /*! * Default constructor */ - Glonass_GNAV_Ephemeris(); + Glonass_Gnav_Ephemeris(); }; #endif diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc new file mode 100644 index 000000000..0dc90d25d --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -0,0 +1,577 @@ +/*! +m * \file glonass_gnav_navigation_message.cc + * \brief Implementation of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1) + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "glonass_gnav_navigation_message.h" +#include +#include +#include + + +void Glonass_Gnav_Navigation_Message::reset() +{ + b_valid_ephemeris_set_flag = false; + double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + d_TOW_SF1 = 0; //!< Time of GPS Week from HOW word of Subframe 1 [s] + d_TOW_SF2 = 0; //!< Time of GPS Week from HOW word of Subframe 2 [s] + d_TOW_SF3 = 0; //!< Time of GPS Week from HOW word of Subframe 3 [s] + d_TOW_SF4 = 0; //!< Time of GPS Week from HOW word of Subframe 4 [s] + d_TOW_SF5 = 0; //!< Time of GPS Week from HOW word of Subframe 5 [s] + + d_m = 0.0; //!< String number within frame [dimensionless] + d_t_k = 0.0; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] + d_t_b = 0.0; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] + d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless] + d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] + d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + d_B_n = 0.0; //!< Health flag [dimensionless] + d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] + d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] + d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] + d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation + d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] + d_E_n = 0.0; //!< Characterises "age" of a current information [days] + d_P_1 = 0.0; //!< Flag of the immediate data updating. + d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + d_l_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + + // Almanac and Not Inmediate Information + d_tau_c = 0.0; //!< GLONASS time scale correction to UTC(SU) time. [s] + d_tau_gps = 0.0; //!< Correction to GPS time to GLONASS time [day] + d_N_4 = 0.0; //!< Four year interval number starting from 1996 [4 year interval] + d_N_A = 0.0; //!< Calendar day number within the four-year period beginning since the leap year [days] + d_n_A = 0.0; //!< Conventional number of satellite within GLONASS space segment [dimensionless] + d_H_n_A = 0.0; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] + d_lambda_n_A = 0.0; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] + d_t_lambda_n_A = 0.0; //!< Time of first ascending node passage [s] + d_Delta_i_n_A = 0.0; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] + d_Delta_T_n_A = 0.0; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period] + d_Delta_T_n_A_dot = 0.0; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2] + d_epsilon_n_A = 0.0; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless] + d_omega_n_A = 0.0; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles] + d_M_n_A = 0.0; //!< Type of satellite n_A [dimensionless] + d_B1 = 0.0; //!< Coefficient to determine DeltaUT1 [s] + d_B2 = 0.0; //!< Coefficient to determine DeltaUT1 [s/msd] + d_KP = 0.0; //!< Notification on forthcoming leap second correction of UTC [dimensionless] + d_tau_n_A = 0.0; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] + d_C_n_A = 0.0; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + + std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus + + + /*! \brief If true, enhanced level of integrity assurance. + * + * If false, indicates that the conveying signal is provided with the legacy level of integrity assurance. + * That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound + * value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less + * than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of + * integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73 + * times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an + * accompanying alert, is less than 1E-8 per hour. + */ + b_integrity_status_flag = false; + b_alert_flag = false; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk. + b_antispoofing_flag = false; //!< If true, the AntiSpoofing mode is ON in that SV + + // Clock terms + d_satClkCorr = 0.0; // Satellite clock error + d_dtr = 0.0; // Relativistic clock correction term + d_satClkDrift = 0.0; // Satellite clock drift + + // satellite identification info + int i_channel_ID = 0; + int i_satellite_freq_channel = 0; //!< SV PRN NUMBER + + // time synchro + d_subframe_timestamp_ms = 0; //[ms] + + // UTC parameters + bool flag_utc_model_valid = false; //!< If set, it indicates that the UTC model parameters are filled + + // satellite positions + d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + // Satellite acceleration + d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + + auto gnss_sat = Gnss_Satellite(); + std::string _system ("GLONASS"); + //TODO SHould number of channels be hardcoded? + for(unsigned int i = 1; i < 14; i++) + { + satelliteBlock[i] = gnss_sat.what_block(_system, i); + } +} + + +Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() +{ + reset(); +} + + +bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) +{ + bool value; + + if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) + { + value = true; + } + else + { + value = false; + } + return value; +} + + +unsigned long int Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector> parameter) +{ + unsigned long int value = 0; + int num_of_slices = parameter.size(); + for (int i = 0; i < num_of_slices; i++) + { + for (int j = 0; j < parameter[i].second; j++) + { + value <<= 1; //shift left + if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) + { + value += 1; // insert the bit + } + } + } + return value; +} + + +signed long int Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) +{ + signed long int value = 0; + int num_of_slices = parameter.size(); + // Discriminate between 64 bits and 32 bits compiler + int long_int_size_bytes = sizeof(signed long int); + if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system + { + // read the MSB and perform the sign extension + if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) + { + value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable + } + else + { + value &= 0; + } + + for (int i = 0; i < num_of_slices; i++) + { + for (int j = 0; j < parameter[i].second; j++) + { + value <<= 1; //shift left + value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable) + if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) + { + value += 1; // insert the bit + } + } + } + } + else // we assume we are in a 32 bits system + { + // read the MSB and perform the sign extension + if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) + { + value ^= 0xFFFFFFFF; + } + else + { + value &= 0; + } + + for (int i = 0; i < num_of_slices; i++) + { + for (int j = 0; j < parameter[i].second; j++) + { + value <<= 1; //shift left + value &= 0xFFFFFFFE; //reset the corresponding bit + if (bits[GPS_SUBFRAME_BITS - parameter[i].first - j] == 1) + { + value += 1; // insert the bit + } + } + } + } + return value; +} + +int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string, int frame_ID) +{ + int string_ID = 0; + + // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE + std::bitset string_bits(std::string(frame_string)); + + string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); + + // Decode all 15 string messages + switch (string_ID) + { + case 1: + //--- It is string 1 ----------------------------------------------- + gnav_ephemeris.d_P_1 = static_cast(read_navigation_unsigned(string_bits, P1)); + gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K)); + gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)); + gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)); + gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)); + + break; + + case 2: + //--- It is string 2 ----------------------------------------------- + gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); + gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); + gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B)); + gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT)); + gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)); + gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)); + + break; + + case 3: + // --- It is string 3 ---------------------------------------------- + gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); + gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)); + gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); + gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)); + gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)); + gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)); + + break; + + case 4: + // --- It is subframe 4 -------------------------------------------- + // TODO signed vs unsigned reading from datasheet + gnav_ephemeris.d_tau_n = static_cast(read_navigation_unsigned(string_bits, TAU_N)); + gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)); + gnav_ephemeris.d_E_n = static_cast(read_navigation_unsigned(string_bits, E_N)); + gnav_ephemeris.d_P_4 = static_cast(read_navigation_unsigned(string_bits, P4)); + gnav_ephemeris.d_F_T = static_cast(read_navigation_unsigned(string_bits, F_T)); + gnav_ephemeris.d_N_T = static_cast(read_navigation_unsigned(string_bits, N_T)); + gnav_ephemeris.d_n = static_cast(read_navigation_unsigned(string_bits, N)); + gnav_ephemeris.d_M = static_cast(read_navigation_unsigned(string_bits, M)); + + break; + + case 5: + // --- It is string 5 ---------------------------------------------- + // TODO signed vs unsigned reading from datasheet + d_N_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); + d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + + break; + + case 6: + // --- It is string 6 ---------------------------------------------- + // TODO signed vs unsigned reading from datasheet + i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + + flag_almanac_str_6 = true; + + break; + + case 7: + // --- It is string 7 ---------------------------------------------- + if (flag_almanac_str_6 == true) + { + // TODO signed vs unsigned reading from datasheet + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + + flag_almanac_str_7 = true; + } + + + break; + case 8: + // --- It is string 8 ---------------------------------------------- + // TODO signed vs unsigned reading from datasheet + i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + + flag_almanac_str_8 = true; + + break; + + case 9: + // --- It is string 9 ---------------------------------------------- + if (flag_almanac_str_8 == true) + { + // TODO signed vs unsigned reading from datasheet + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + + flag_almanac_str_9 = true; + } + case 10: + // --- It is string 8 ---------------------------------------------- + // TODO signed vs unsigned reading from datasheet + i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + + flag_almanac_str_10 = true; + + break; + + case 11: + // --- It is string 9 ---------------------------------------------- + if (flag_almanac_str_10 == true) + { + // TODO signed vs unsigned reading from datasheet + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + + flag_almanac_str_11 = true; + } + break; + case 12: + // --- It is string 8 ---------------------------------------------- + // TODO signed vs unsigned reading from datasheet + i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + + flag_almanac_str_12 = true; + + break; + + case 13: + // --- It is string 9 ---------------------------------------------- + if (flag_almanac_str_12 == true) + { + // TODO signed vs unsigned reading from datasheet + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + + flag_almanac_str_13 = true; + } + case 14: + // --- It is string 8 ---------------------------------------------- + if( frame_number == 5) + { + gnav_utc_model.B1 = static_cast(read_navigation_unsigned(string_bits, B1)); + gnav_utc_model.B2 = static_cast(read_navigation_unsigned(string_bits, B2)); + } + else + { + // TODO signed vs unsigned reading from datasheet + i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + + flag_almanac_str_14 = true; + } + + + break; + + case 15: + // --- It is string 9 ---------------------------------------------- + if (frame_number != 5 and flag_almanac_str_14 = true ) + { + // TODO signed vs unsigned reading from datasheet + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + + flag_almanac_str_15 = true; + } + default: + break; + } // switch subframeID ... + + return subframe_ID; +} + + + + +double Glonass_Gnav_Navigation_Message::utc_time(const double glonass_time_corrected) const +{ + double t_utc; + + t_utc = glonass_time_corrected + 3*3600 + d_tau_c; + return t_utc; +} + + + +Glonass_Gnav_Ephemeris Glonass_Gnav_Navigation_Message::get_ephemeris() +{ + Glonass_Gnav_Ephemeris ephemeris; + + ephemeris.i_satellite_freq_channel = i_satellite_freq_channel; + ephemeris.d_m = d_m; + ephemeris.d_t_k = d_t_k; + ephemeris.d_t_b = d_t_b; + ephemeris.d_M = d_M; + ephemeris.d_gamma_n = d_gamma_n; + ephemeris.d_tau_n = d_tau_n; + // satellite positions + ephemeris.d_satpos_X = d_satpos_X; + ephemeris.d_satpos_Y = d_satpos_Y; + ephemeris.d_satpos_Z = d_satpos_Z; + // Satellite velocity + ephemeris.d_satvel_X = d_satvel_X; + ephemeris.d_satvel_Y = d_satvel_Y; + ephemeris.d_satvel_Z = d_satvel_Z; + // Satellite acceleration + ephemeris.d_satacc_X = d_satacc_X; + ephemeris.d_satacc_Y = d_satacc_Y; + ephemeris.d_satacc_Z = d_satacc_Z; + ephemeris.d_B_n = d_B_n; + ephemeris.d_P = d_P; + ephemeris.d_N_T = d_N_T; + ephemeris.d_F_T = d_F_T; + ephemeris.d_n = d_n; + ephemeris.d_Delta_tau_n = d_Delta_tau_n; + ephemeris.d_E_n = d_E_n; + ephemeris.d_P_1 = d_P_1; + ephemeris.d_P_2 = d_P_2; + ephemeris.d_P_3 = d_P_3; + ephemeris.d_P_4 = d_P_4; + ephemeris.d_l_n = d_l_n; + + // clock terms derived from ephemeris data + ephemeris.d_satClkDrift = d_satClkDrift; + ephemeris.d_dtr = d_dtr; + + return ephemeris; +} + + +Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model() +{ + Gps_Utc_Model utc_model; + utc_model.valid = flag_utc_model_valid; + // UTC parameters + utc_model.d_A1 = d_A1; + utc_model.d_A0 = d_A0; + utc_model.d_t_OT = d_t_OT; + utc_model.i_WN_T = i_WN_T; + utc_model.d_DeltaT_LS = d_DeltaT_LS; + utc_model.i_WN_LSF = i_WN_LSF; + utc_model.i_DN = i_DN; + utc_model.d_DeltaT_LSF = d_DeltaT_LSF; + // warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue + flag_utc_model_valid = false; + return utc_model; +} + + +bool Glonass_Gnav_Navigation_Message::satellite_validation() +{ + bool flag_data_valid = false; + b_valid_ephemeris_set_flag = false; + + // First Step: + // check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception + // and check if the data have been filled (!=0) + if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0) + { + if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1) + { + flag_data_valid = true; + b_valid_ephemeris_set_flag = true; + } + } + return flag_data_valid; +} diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h new file mode 100644 index 000000000..0098f6d9a --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -0,0 +1,203 @@ +/*! + * \file glonass_gnav_navigation_message.h + * \brief Interface of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1) + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_ +#define GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_ + + +#include +#include +#include +#include +#include +#include "GLONASS_L1_CA.h" +#include "glonass_gnav_ephemeris.h" +//#include "gps_iono.h" +#include "glonass_gnav_almanac.h" +#include "glonass_gnav_utc_model.h" + + + +/*! + * \brief This class decodes a GLONASS GNAV Data message as described in GLONASS ICD (Edition 5.1) + * + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdfdescribed in IS-GPS-200E + * + */ +class Glonass_Gnav_Navigation_Message +{ +private: + unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); + signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); + bool read_navigation_bool(std::bitset bits, const std::vector> parameter); + bool _CRC_test(std::bitset bits,boost::uint32_t checksum); + + unsigned int get_frame_number(unsigned int satellite_slot_number); + +public: + bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check + int Page_type_time_stamp; + int flag_even_word; + std::string page_Even; + bool flag_CRC_test; + + Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded + Glonass_Gnav_Iono gnav_iono; //!< Iono corrections information + Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information + Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites + + //!< Ephmeris Flags + bool flag_all_ephemeris; //!< Flag indicating that all strings containing ephemeris have been received + bool flag_ephemeris_str_1; //!< Flag indicating that ephemeris 1/4 (word 1) have been received + bool flag_ephemeris_str_2; //!< Flag indicating that ephemeris 2/4 (word 2) have been received + bool flag_ephemeris_str_3; //!< Flag indicating that ephemeris 3/4 (word 3) have been received + bool flag_ephemeris_str_4; //!< Flag indicating that ephemeris 4/4 (word 4) have been received + + bool flag_iono_and_GST; //!< Flag indicating that ionospheric and GST parameters (word 5) have been received + bool flag_TOW_5; + bool flag_TOW_6; + bool flag_TOW_set; //!< it is true when page 5 or page 6 arrives + bool flag_utc_model; //!< Flag indicating that utc model parameters (word 6) have been received + + bool flag_all_almanac; //!< Flag indicating that all almanac have been received + bool flag_almanac_str_6; //!< Flag indicating that almanac 1/4 (word 7) have been received + bool flag_almanac_str_7; //!< Flag indicating that almanac 2/4 (word 8) have been received + bool flag_almanac_str_8; //!< Flag indicating that almanac 3/4 (word 9) have been received + bool flag_almanac_str_9; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_almanac_str_10; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_almanac_str_11; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_almanac_str_12; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_almanac_str_13; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_almanac_str_14; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_almanac_str_15; //!< Flag indicating that almanac 4/4 (word 10) have been received + + //broadcast orbit 1 + //TODO Need to send the information regarding the frame number + double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] + double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s] + double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s] + double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s] + double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s] + + // Clock terms + double d_satClkCorr; // Satellite clock error + double d_dtr; // Relativistic clock correction term + double d_satClkDrift; // Satellite clock drift + + // satellite identification info + int i_channel_ID; + unsigned int i_satellite_PRN; //!< SV PRN Number + int i_satellite_freq_channel; //!< SV Frequency Slot Number + int i_satellite_slot_number; //!< SV Orbit Slot Number + + // UTC parameters + bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled + + // satellite positions + double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + // Satellite velocity + double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + // Satellite acceleration + double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + + /*! + * \brief Reset GLONASS GNAV Navigation Information + */ + void reset(); + + /*! + * \brief Obtain a GLONASS GNAV SV Ephemeris class filled with current SV data + */ + Glonass_Gnav_Ephemeris get_ephemeris(); + + /*! + // TODO Should I keep this function ? + * \brief Obtain a GPS ionospheric correction parameters class filled with current SV data + */ + Glonass_Gnav_Iono get_iono(); + + /*! + * \brief Obtain a GLONASS GNAV UTC model parameters class filled with current SV data + */ + Glonass_Gnav_Utc_Model get_utc_model(); + + /* + * \brief Returns a Galileo_Almanac object filled with the latest navigation data received + */ + Glonass_Gnav_Almanac get_almanac(); + + /* + * \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed + */ + bool have_new_ephemeris(); + + /* + * \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed + */ + bool have_new_iono(); + + /* + * \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed + */ + bool have_new_utc_model(); + + /* + * \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed + */ + bool have_new_almanac(); + + + + /*! + * \brief Decodes the GLONASS GNAV frame + */ + int string_decoder(char *string, int frame_ID); + + /*! + * \brief Computes the Coordinated Universal Time (UTC) and returns it in [s] + */ + double utc_time(const double gpstime_corrected) const; + + /*! + * Default constructor + */ + Glonass_Gnav_Navigation_Message(); +}; + +#endif diff --git a/src/core/system_parameters/glonass_gnav_utc_model.cc b/src/core/system_parameters/glonass_gnav_utc_model.cc index 921195298..55ca5ccee 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.cc +++ b/src/core/system_parameters/glonass_gnav_utc_model.cc @@ -35,6 +35,11 @@ Glonass_Gnav_Utc_Model::Glonass_Gnav_Utc_Model() { valid = false; d_tau_c = 0.0; + d_tau_gps = 0.0; + d_N_4 = 0.0; + d_N_A = 0.0; + d_B1 = 0.0; + d_B2 = 0.0; } double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected) @@ -43,6 +48,6 @@ double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected) // GLONASS Time is relative to UTC Moscow, so we simply add its time difference t_utc = glonass_time_corrected + 3*3600 + d_tau_c; - + return t_utc; } diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index b08c4bdce..51d47f6c3 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -45,7 +45,30 @@ class Glonass_Gnav_Utc_Model public: bool valid; // UTC - double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] + double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] + double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] + double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] + double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days] + double d_B1; //!< Coefficient to determine DeltaUT1 [s] + double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd] + + 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. + */ + void serialize(Archive& archive, const unsigned int version) + { + using boost::serialization::make_nvp; + if(version){}; + + archive & make_nvp("valid",valid); + archive & make_nvp("d_tau_c", d_tau_c); + archive & make_nvp("d_tau_gps", d_tau_gps); + archive & make_nvp("d_N_4", d_N_4); + archive & make_nvp("d_N_A", d_N_A); + archive & make_nvp("d_B1", d_B1); + archive & make_nvp("d_B2", d_B2); + } /*! * Default constructor @@ -58,18 +81,6 @@ public: */ 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 ephemeris data on disk file. - */ - void serialize(Archive& archive, const unsigned int version) - { - using boost::serialization::make_nvp; - if(version){}; - archive & make_nvp("valid",valid); - archive & make_nvp("d_tau_c",d_tau_c); - } - }; #endif diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 60b33a4d0..36c5620d7 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -59,6 +59,11 @@ #include "sbas_ephemeris.h" +#include "glonass_gnav_ephemeris.h" +#include "glonass_gnav_almanac.h" +#include "glonass_gnav_iono.h" +#include "glonass_gnav_utc_model.h" + using google::LogMessage; DECLARE_string(log_dir); @@ -139,7 +144,8 @@ DECLARE_string(log_dir); #endif #endif - +#include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" +#include "unit-tests/system-parameters/glonass_gnav_almanac_test.cc" // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc new file mode 100644 index 000000000..650daa341 --- /dev/null +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc @@ -0,0 +1,76 @@ +/*! + * \file code_generation_test.cc + * \brief This file implements tests for the generation of complex exponentials. + * \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include "gps_sdr_signal_processing.h" +#include "gnss_signal_processing.h" + + + +#include +#include +#include "gnss_signal_processing.h" +#include "glonass_gnav_almanac.h" + +// See A 3.2.3 +TEST(GlonassGnavEphemerisTest, SatellitePosition) +{ + double N_i = 615; // [days] + double t_i = 33300.0 // [seconds] + double Xoi = 10947.021572; // [km] + double Yoi = 13078.978287; // [km] + double Zoi = 18922.063362; // [km] + double Vxoi = -3.375497 // [m/s] + double Vyoi = -0.161453; // [Кm/s] + double Vzoi = 2.060844; // [Кm/s] + + Glonass_Gnav_Ephemeris gnav_almanac; + + gnav_almanac.d_N_A = 615; // [days] + gnav_almanac.d_lambda_n_A = -0.189986229; // [half cycles] + gnav_almanac.d_t_lambda_n_A = 27122.09375; // [second] + gnav_almanac.d_Delta_i_n_A = 0.011929512; // [half cycle] + gnav_almanac.d_Delta_T_n_A = -2655.76171875; // [seconds] + gnav_almanac.d_Delta_T_n_A_dot = 0.000549316; // [Secjnds/cycle2] + gnav_almanac.d_epsilon_n_A = 0.001482010; // [unitless] + gnav_almanac.d_omega_n_A = 0.440277100; // [Half cycle] + + gnav_almanac.satellite_position(N_i, t_i); + + ASSERT_TRUE(gnav_almanac.d_satpos_Xo - Xoi < DBL_EPSILON ); + ASSERT_TRUE(gnav_almanac.d_satpos_Yo - Yoi < DBL_EPSILON ); + ASSERT_TRUE(gnav_almanac.d_satpos_Zo - Zoi < DBL_EPSILON ); + ASSERT_TRUE(gnav_almanac.d_satvel_Xo - Vxoi < DBL_EPSILON ); + ASSERT_TRUE(gnav_almanac.d_satvel_Yo - Vyoi < DBL_EPSILON ); + ASSERT_TRUE(gnav_almanac.d_satvel_Zo - Vzoi < DBL_EPSILON ); +} diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc new file mode 100644 index 000000000..44497f114 --- /dev/null +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc @@ -0,0 +1,54 @@ +/*! + * \file code_generation_test.cc + * \brief This file implements tests for the generation of complex exponentials. + * \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include "gnss_signal_processing.h" +#include "glonass_gnav_ephemeris.h" + + +TEST(GlonassGnavEphemerisTest, SatellitePosition) +{ + Glonass_Gnav_Ephemeris gnav_eph; + + gnav_eph.d_Xn = 12317.934082000; + gnav_eph.d_Yn = -2245.13232422; + gnav_eph.d_Zn = 22212.8173828; + gnav_eph.d_VXn = -1.25356674194; + gnav_eph.d_VYn = 2.774200439450; + gnav_eph.d_VZn = 0.9808206558230000; + gnav_eph.d_AXn = -0.931322574616e-9; + gnav_eph.d_AYn = 0.0000000000000000; + gnav_eph.d_AZn = -0.186264514923e-8; + + gnav_eph.simplified_satellite_position(60); +} From 6c8a29d3d46f40967b73e26937dacf92ddccfb43 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Thu, 6 Jul 2017 23:02:50 -0700 Subject: [PATCH 21/48] writing code for crc computation in GLONASS processing --- src/core/system_parameters/GLONASS_L1_CA.h | 9 ++ .../glonass_gnav_navigation_message.cc | 111 +++++++++++++++++- 2 files changed, 119 insertions(+), 1 deletion(-) diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index cc5ff2db4..1b5eeefc9 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -107,6 +107,15 @@ const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] const int GLONASS_GNAV_FRAME_MS = 30000; //!< Subframe duration [seconds] const int GLONASS_GNAV_STRING_BITS = 115; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] +const std::vector GLONASS_GNAV_CRC_I_INDEX {9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84}; +const std::vector GLONASS_GNAV_CRC_J_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; +const std::vector GLONASS_GNAV_CRC_K_INDEX {10, 11, 12, 16, 17, 18, 19, 23, 24, 25, 26, 31, 32, 33, 34 38, 39, 40, 41, 46, 47, 48, 49, 54, 55, 56, 57, 62, 63, 64, 65, 69, 70, 71, 72, 77, 78, 79, 80, 85}; +const std::vector GLONASS_GNAV_CRC_L_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; +const std::vector GLONASS_GNAV_CRC_M_INDEX {20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 81, 82, 83, 84, 85}; +const std::vector GLONASS_GNAV_CRC_N_INDEX {35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85}; +const std::vector GLONASS_GNAV_CRC_P_INDEX {66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85}; +const std::vector GLONASS_GNAV_CRC_Q_INDEX {9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85}; + // GLONASS GNAV NAVIGATION MESSAGE STRUCTURE // NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix II) diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 0dc90d25d..7fcc00cad 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -145,6 +145,81 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() reset(); } +bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data_bits, std::bitset hamming_code_bits ) +{ + int sum; + + //!< Compute C1 term + sum = 0; + for(int i = 0; i < GLONASS_GNAV_CRC_I_INDEX.size; i++) + { + sum += data_bits[GLONASS_GNAV_CRC_I_INDEX[i]] + } + C1 = hamming_code_bits[0]^fmod(sum,2); + + //!< Compute C2 term + sum = 0; + for(int j = 0; j < GLONASS_GNAV_CRC_J_INDEX.size; j++) + { + sum += data_bits[GLONASS_GNAV_CRC_J_INDEX[j]] + } + C2 = hamming_code_bits[1]^fmod(sum,2); + + //!< Compute C3 term + sum = 0; + for(int k = 0; k < GLONASS_GNAV_CRC_K_INDEX.size; k++) + { + sum += data_bits[GLONASS_GNAV_CRC_K_INDEX[k]] + } + C3 = hamming_code_bits[2]^fmod(sum,2); + + //!< Compute C4 term + sum = 0; + for(int l = 0; l < GLONASS_GNAV_CRC_L_INDEX.size; l++) + { + sum += data_bits[GLONASS_GNAV_CRC_L_INDEX[l]] + } + C4 = hamming_code_bits[3]^fmod(sum,2); + + //!< Compute C5 term + sum = 0; + for(int m = 0; m < GLONASS_GNAV_CRC_M_INDEX.size; m++) + { + sum += data_bits[GLONASS_GNAV_CRC_M_INDEX[m]] + } + C5 = hamming_code_bits[4]^fmod(sum,2); + + //!< Compute C6 term + sum = 0; + for(int n = 0; n < GLONASS_GNAV_CRC_N_INDEX.size; n++) + { + sum += data_bits[GLONASS_GNAV_CRC_N_INDEX[n]] + } + C6 = hamming_code_bits[5]^fmod(sum,2); + + //!< Compute C7 term + sum = 0; + for(int p = 0; p < GLONASS_GNAV_CRC_P_INDEX.size; p++) + { + sum += data_bits[GLONASS_GNAV_CRC_P_INDEX[p]] + } + C7 = hamming_code_bits[6]^fmod(sum,2); + + //!< Compute C8 term + sum = 0; + for(int q = 0; q < GLONASS_GNAV_CRC_Q_INDEX.size; q++) + { + sum += data_bits[GLONASS_GNAV_CRC_Q_INDEX[q]] + } + C8 = hamming_code_bits[7]^fmod(sum,2); + + if isempty(find(C,1)) || (length(find(C(1,1:7))) == 1 && C(1,8) == 1) + status = 1; + else + status = 0; + end +} + bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) { @@ -240,12 +315,46 @@ signed long int Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bit return value; } -int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string, int frame_ID) +unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int satellite_slot_number) +{ + unsigned int frame_ID = 0; + + if(satellite_slot_number >= 1 and satellite_slot_number <= 5 ) + { + frame_ID = 1; + } + else if(satellite_slot_number >= 6 and satellite_slot_number <= 10 ) + { + frame_ID = 2; + } + else if(satellite_slot_number >= 11 and satellite_slot_number <= 15 ) + { + frame_ID = 3; + } + else if(satellite_slot_number >= 16 and satellite_slot_number <= 20 ) + { + frame_ID = 4; + } + else if(satellite_slot_number >= 21 and satellite_slot_number <= 24 ) + { + frame_ID = 5; + } + else + { + //TODO Find print statement and make it an error + frame_ID = 0; + } +} + +int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) { int string_ID = 0; + int frame_ID = 0; // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE std::bitset string_bits(std::string(frame_string)); + std::bitset data_bits(std::string(frame_string), 0, 77); + std::bitset hamming_code_bits(std::string(frame_string), 77, 8); string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); From b34a82a949a481fbecc0df429c509a941bd1039e Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 9 Jul 2017 23:00:49 -0700 Subject: [PATCH 22/48] Adding code for telemetry decoder algorithms --- .../glonass_l1_ca_telemetry_decoder.cc | 102 ++++ .../glonass_l1_ca_telemetry_decoder.h | 91 ++++ .../glonass_l1_ca_telemetry_decoder_cc.cc | 441 ++++++++++++++++++ .../glonass_l1_ca_telemetry_decoder_cc.h | 114 +++++ src/core/system_parameters/GLONASS_L1_CA.h | 6 +- .../glonass_gnav_navigation_message.cc | 339 +++++++------- .../glonass_gnav_navigation_message.h | 93 ++-- 7 files changed, 952 insertions(+), 234 deletions(-) create mode 100644 src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc create mode 100644 src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h create mode 100644 src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc create mode 100644 src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h diff --git a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc new file mode 100644 index 000000000..868fb3304 --- /dev/null +++ b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc @@ -0,0 +1,102 @@ +/*! + * \file glonass_l1_ca_telemetry_decoder.cc + * \brief Implementation of an adapter of a GLONASS L1 C/A NAV data decoder block + * to a TelemetryDecoderInterface + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "glonass_l1_ca_telemetry_decoder.h" +#include +#include +#include "concurrent_queue.h" +#include "glonass_gnav_ephemeris.h" +#include "glonass_gnav_almanac.h" +#include "glonass_gnav_iono.h" +#include "glonass_gnav_utc_model.h" +#include "configuration_interface.h" + +using google::LogMessage; + +GlonassL1CaTelemetryDecoder::GlonassL1CaTelemetryDecoder(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams) : + role_(role), + in_streams_(in_streams), + out_streams_(out_streams) +{ + std::string default_dump_filename = "./navigation.dat"; + DLOG(INFO) << "role " << role; + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); + // make telemetry decoder object + telemetry_decoder_ = glonass_l1_ca_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me + DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; + + DLOG(INFO) << "global navigation message queue assigned to telemetry_decoder ("<< telemetry_decoder_->unique_id() << ")"; + channel_ = 0; +} + + +GlonassL1CaTelemetryDecoder::~GlonassL1CaTelemetryDecoder() +{} + + +void GlonassL1CaTelemetryDecoder::set_satellite(Gnss_Satellite satellite) +{ + satellite_ = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + telemetry_decoder_->set_satellite(satellite_); + DLOG(INFO) << "TELEMETRY DECODER: satellite set to " << satellite_; +} + + +void GlonassL1CaTelemetryDecoder::connect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to connect internally + DLOG(INFO) << "nothing to connect internally"; +} + + +void GlonassL1CaTelemetryDecoder::disconnect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to disconnect +} + + +gr::basic_block_sptr GlonassL1CaTelemetryDecoder::get_left_block() +{ + return telemetry_decoder_; +} + + +gr::basic_block_sptr GlonassL1CaTelemetryDecoder::get_right_block() +{ + return telemetry_decoder_; +} diff --git a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h new file mode 100644 index 000000000..d9d883a71 --- /dev/null +++ b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h @@ -0,0 +1,91 @@ +/*! + * \file glonass_l1_ca_telemetry_decoder.h + * \brief Interface of an adapter of a GLONASS L1 C/A NAV data decoder block + * to a TelemetryDecoderInterface + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_L1_CA_TELEMETRY_DECODER_H_ +#define GNSS_SDR_GLONASS_L1_CA_TELEMETRY_DECODER_H_ + +#include +#include "telemetry_decoder_interface.h" +#include "glonass_l1_ca_telemetry_decoder_cc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a NAV data decoder for GLONASS L1 C/A + */ +class GlonassL1CaTelemetryDecoder : public TelemetryDecoderInterface +{ +public: + GlonassL1CaTelemetryDecoder(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~GlonassL1CaTelemetryDecoder(); + std::string role() + { + return role_; + } + + //! Returns "GLONASS_L1_CA_Telemetry_Decoder" + std::string implementation() + { + return "GLONASS_L1_CA_Telemetry_Decoder"; + } + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + void set_satellite(Gnss_Satellite satellite); + void set_channel(int channel){telemetry_decoder_->set_channel(channel);} + void reset() + { + return; + } + size_t item_size() + { + return 0; + } + +private: + glonass_l1_ca_telemetry_decoder_cc_sptr telemetry_decoder_; + Gnss_Satellite satellite_; + int channel_; + bool dump_; + std::string dump_filename_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; +}; + +#endif diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc new file mode 100644 index 000000000..6318977f5 --- /dev/null +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -0,0 +1,441 @@ +/*! + * \file galileo_e1b_telemetry_decoder_cc.cc + * \brief Implementation of a Galileo INAV message demodulator block + * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com + * \author Javier Arribas 2013. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "galileo_e1b_telemetry_decoder_cc.h" +#include +#include +#include +#include +#include +#include +#include "control_message_factory.h" +#include "gnss_synchro.h" +#include "convolutional.h" + + +#define CRC_ERROR_LIMIT 6 + +using google::LogMessage; + + +glonass_l1_ca_telemetry_decoder_cc_sptr +glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump) +{ + return glonass_l1_ca_telemetry_decoder_cc_sptr(new glonass_l1_ca_telemetry_decoder_cc(satellite, dump)); +} + + +glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( + Gnss_Satellite satellite, + bool dump) : + gr::block("glonass_l1_ca_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry Bit transition synchronization port out + this->message_port_register_out(pmt::mp("preamble_timestamp_s")); + // Ephemeris data port out + this->message_port_register_out(pmt::mp("telemetry")); + // initialize internal vars + d_dump = dump; + d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + LOG(INFO) << "Initializing GLONASS L1CA TELEMETRY PROCESSING"; + // TODO. WHAT IS THIS? + d_samples_per_symbol = ( GLONASS_L1_CODE_CHIP_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS ) / GLONASS_L1_CA_SYMBOL_RATE_BPS; + + // set the preamble + unsigned short int preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS] = GLONASS_GNAV_PREAMBLE; + d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol; + + memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, GLONASS_GNAV_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int)); + + // preamble bits to sampled symbols + d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble); + int n = 0; + for (int i = 0; i < GLONASS_GNAV_PREAMBLE_LENGTH_BITS; i++) + { + for (unsigned int j = 0; j < d_samples_per_symbol; j++) + { + if (d_preambles_bits[i] == 1) + { + d_preambles_symbols[n] = 1; + } + else + { + d_preambles_symbols[n] = -1; + } + n++; + } + } + d_sample_counter = 0; + d_stat = 0; + d_preamble_index = 0; + + d_flag_frame_sync = false; + + d_flag_parity = false; + d_TOW_at_current_symbol = 0; + delta_t = 0; + d_CRC_error_counter = 0; + flag_even_word_arrived = 0; + d_flag_preamble = false; + d_channel = 0; + flag_TOW_set = false; +} + + +glonass_l1_ca_telemetry_decoder_cc::~glonass_l1_ca_telemetry_decoder_cc() +{ + delete d_preambles_symbols; + d_dump_file.close(); +} + + +void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols,int frame_length) +{ + double page_part_symbols_deint[frame_length]; + + // 2. Viterbi decoder + // 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder) + // 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180� + for (int i = 0; i < frame_length; i++) + { + if ((i + 1) % 2 == 0) + { + page_part_symbols_deint[i] = -page_part_symbols_deint[i]; + } + } + + int page_part_bits[frame_length/2]; + viterbi_decoder(page_part_symbols_deint, page_part_bits); + + // 3. Call the Galileo page decoder + std::string page_String; + for(int i = 0; i < (frame_length/2); i++) + { + if (page_part_bits[i] > 0) + { + page_String.push_back('1'); + } + else + { + page_String.push_back('0'); + } + } + + d_nav.decode_string(page_String); + + if(d_nav.flag_CRC_test == true) + { + LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite; + std::cout << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite << std::endl; + } + else + { + std::cout << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite << std::endl; + LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite; + } + + + + // 4. Push the new navigation data to the queues + if (d_nav.have_new_ephemeris() == true) + { + // get object for this SV (mandatory) + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); + + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + + } + if (d_nav.have_new_utc_model() == true) + { + // get object for this SV (mandatory) + 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)); + } + if (d_nav.have_new_almanac() == true) + { + std::shared_ptr tmp_obj= std::make_shared(d_nav.get_almanac()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + //debug + std::cout << "GLONASS GNAV almanac received!" << std::endl; + DLOG(INFO) << "Current parameters:"; + DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; + DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; + delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); + DLOG(INFO) << "delta_t=" << delta_t << "[s]"; + } +} + + + + +int glonass_l1_ca_telemetry_decoder_cc::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) +{ + int corr_value = 0; + int preamble_diff = 0; + + Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + const Gnss_Synchro **in = (const Gnss_Synchro **) &input_items[0]; //Get the input samples pointer + + Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block + //1. Copy the current tracking output + current_symbol = in[0][0]; + d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue + d_sample_counter++; //count for the processed samples + consume_each(1); + + d_flag_preamble = false; + unsigned int required_symbols=GLONASS_GNAV_PAGE_SYMBOLS+d_symbols_per_preamble; + + if (d_symbol_history.size()>required_symbols) + { + //******* preamble correlation ******** + for (int i = 0; i < d_symbols_per_preamble; i++) + { + if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping + { + corr_value -= d_preambles_symbols[i]; + } + else + { + corr_value += d_preambles_symbols[i]; + } + } + } + + //******* frame sync ****************** + if (d_stat == 0) //no preamble information + { + if (abs(corr_value) >= d_symbols_per_preamble) + { + d_preamble_index = d_sample_counter;//record the preamble sample stamp + LOG(INFO) << "Preamble detection for GLONASS L1 C/A SAT " << this->d_satellite; + d_stat = 1; // enter into frame pre-detection status + } + } + else if (d_stat == 1) // posible preamble lock + { + if (abs(corr_value) >= d_symbols_per_preamble) + { + //check preamble separation + preamble_diff = d_sample_counter - d_preamble_index; + if (abs(preamble_diff - GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) == 0) + { + //try to decode frame + LOG(INFO) << "Starting page decoder for GLONASS L1 C/A SAT " << this->d_satellite; + d_preamble_index = d_sample_counter; //record the preamble sample stamp + d_stat = 2; + } + else + { + if (preamble_diff > GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) + { + d_stat = 0; // start again + } + } + } + } + else if (d_stat == 2) + { + if (d_sample_counter == d_preamble_index + GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) + { + // NEW GLONASS string received + // 0. fetch the symbols into an array + int frame_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; + double page_part_symbols[frame_length]; + + //******* SYMBOL TO BIT ******* + if (d_symbol_history.at(0).Flag_valid_symbol_output == true) + { + // extended correlation to bit period is enabled in tracking! + d_symbol_accumulator += d_symbol_history.at(0).Prompt_I; // accumulate the input value in d_symbol_accumulator + d_symbol_accumulator_counter += d_symbol_history.at(0).correlation_length_ms; + } + if (d_symbol_accumulator_counter >= 20) + { + } + + for (int i = 0; i < frame_length; i++) + { + if (corr_value > 0) + { + page_part_symbols[i] = d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now! + + } + else + { + page_part_symbols[i] = -d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now! + } + } + + //call the decoder + decode_string(page_part_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) << " Frame sync SAT " << this->d_satellite << " with preamble start at " + << d_symbol_history.at(0).Tracking_sample_counter << " [samples]"; + } + } + else + { + d_CRC_error_counter++; + d_preamble_index = d_sample_counter; //record the preamble sample stamp + if (d_CRC_error_counter > CRC_ERROR_LIMIT) + { + LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; + d_flag_frame_sync = false; + d_stat = 0; + } + } + } + } + + // UPDATE GNSS SYNCHRO DATA + //2. Add the telemetry decoder information + if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) + //update TOW at the preamble instant + { + if(d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) + { + //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay + d_TOW_at_current_symbol = d_nav.TOW_5 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; + d_nav.flag_TOW_5 = false; + } + + else if(d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) + { + //TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay + d_TOW_at_current_symbol = d_nav.TOW_6 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD;//-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; + d_nav.flag_TOW_6 = false; + } + else + { + //this page has no timing information + d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; + } + } + else //if there is not a new preamble, we define the TOW of the current symbol + { + d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; + } + + //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) + + if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived + { + delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); + } + + if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) + { + current_symbol.Flag_valid_word = true; + } + else + { + current_symbol.Flag_valid_word = false; + } + + current_symbol.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0; + current_symbol.TOW_at_current_symbol_s -=delta_t; //Galileo to GPS TOW + + if(d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = d_TOW_at_current_symbol; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write((char*)&tmp_ulong_int, sizeof(unsigned long int)); + tmp_double = 0; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + + // remove used symbols from history + if (d_symbol_history.size()>required_symbols) + { + d_symbol_history.pop_front(); + } + //3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + + return 1; +} + + +void glonass_l1_ca_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite) +{ + d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; + DLOG(INFO) << "Navigation Satellite set to " << d_satellite; +} + + +void glonass_l1_ca_telemetry_decoder_cc::set_channel(int channel) +{ + d_channel = channel; + LOG(INFO) << "Navigation channel set to " << channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename = "telemetry"; + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h new file mode 100644 index 000000000..bfd9d975e --- /dev/null +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h @@ -0,0 +1,114 @@ +/*! + * \file glonass_l1_ca_telemetry_decoder_cc.h + * \brief Interface of a GLONASS GNAV message demodulator block + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GLONASS_L1_CA_TELEMETRY_DECODER_CC_H +#define GNSS_SDR_GLONASS_L1_CA_TELEMETRY_DECODER_CC_H + +#include +#include +#include +#include "GLONASS_L1_CA.h" +#include "concurrent_queue.h" +#include "gnss_satellite.h" +#include "glonass_gnav_navigation_message.h" +#include "glonass_gnav_ephemeris.h" +#include "glonass_gnav_almanac.h" +#include "glonass_gnav_iono.h" +#include "glonass_gnav_utc_model.h" +#include "gnss_synchro.h" + + + +class glonass_l1_ca_telemetry_decoder_cc; + +typedef boost::shared_ptr glonass_l1_ca_telemetry_decoder_cc_sptr; + +glonass_l1_ca_telemetry_decoder_cc_sptr glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); + +/*! + * \brief This class implements a block that decodes the GNAV data defined in GLONASS ICD + * + */ +class glonass_l1_ca_telemetry_decoder_cc : public gr::block +{ +public: + ~glonass_l1_ca_telemetry_decoder_cc(); + void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN + void set_channel(int channel); //!< Set receiver's channel + + /*! + * \brief This is where all signal processing takes place + */ + 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 glonass_l1_ca_telemetry_decoder_cc_sptr + glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); + glonass_l1_ca_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); + + void decode_word(double *symbols,int frame_length); + + //!< Preamble decoding + unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS]; + int *d_preambles_symbols; + unsigned int d_samples_per_symbol; + int d_symbols_per_preamble; + + //!< Storage for incoming data + std::deque d_symbol_history; + + //!< Variables for internal functionality + long unsigned int d_sample_counter; //!< Sample counter as an index (1,2,3,..etc) indicating number of samples processed + long unsigned int d_preamble_index; //!< Index of sample number where preamble was found + unsigned int d_stat; //!< Status of decoder + bool d_flag_frame_sync; //!< Indicate when a frame sync is achieved + bool d_flag_parity; //!< Flag indicating when parity check was achieved (crc check) + bool d_flag_preamble; + int d_CRC_error_counter; + bool flag_TOW_set; //!< + double delta_t; //!< GPS-GLONASS time offset + + //!< Navigation Message variable + Glonass_Gnav_Navigation_Message d_nav; + + //!< Values to populate gnss synchronization structure + double d_TOW_at_current_symbol; + bool Flag_valid_word; + + //!< Satellite Information and logging capacity + Gnss_Satellite d_satellite; + int d_channel; + bool d_dump; + std::string d_dump_filename; + std::ofstream d_dump_file; +}; + +#endif diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 1b5eeefc9..158dae798 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -84,6 +84,7 @@ const double GLONASS_L1_CA_CODE_RATE_HZ = 0.511e6; //!< GLONASS L1 C/ const double GLONASS_L1_CA_CODE_LENGTH_CHIPS = 511.0; //!< GLONASS L1 C/A code length [chips] const double GLONASS_L1_CA_CODE_PERIOD = 0.001; //!< GLONASS L1 C/A code period [seconds] const double GLONASS_L1_CA_CHIP_PERIOD = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds] +const double GLONASS_L1_CA_SYMBOL_RATE_BPS = 1000; const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) @@ -91,7 +92,6 @@ const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time ( const int GLONASS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING - #define GLONASS_CA_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0} const int GLONASS_CA_PREAMBLE_LENGTH_BITS = 30; const int GLONASS_CA_PREAMBLE_LENGTH_SYMBOLS = 300; @@ -99,9 +99,7 @@ const double GLONASS_CA_PREAMBLE_DURATION_S = 0.3; const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] const int GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT = 10; const int GLONASS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_CA_TELEMETRY_RATE_BITS_SECOND*GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s] - -const int GLONASS_GNAV_WORD_LENGTH = 4; //!< \TODO cHECK this, size is not integer bte size -const int GLONASS_GNAV_FRAME_LENGTH = 40; //!< \TODO GPS_WORD_LENGTH x 10 = 40 bytes +const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame in the GNAV message 15 strings*(85 data bits + 30 time mark bits)[bits] const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] const int GLONASS_GNAV_FRAME_MS = 30000; //!< Subframe duration [seconds] diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 7fcc00cad..db5b84eee 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -37,98 +37,55 @@ m * \file glonass_gnav_navigation_message.cc void Glonass_Gnav_Navigation_Message::reset() { - b_valid_ephemeris_set_flag = false; - double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - d_TOW_SF1 = 0; //!< Time of GPS Week from HOW word of Subframe 1 [s] - d_TOW_SF2 = 0; //!< Time of GPS Week from HOW word of Subframe 2 [s] - d_TOW_SF3 = 0; //!< Time of GPS Week from HOW word of Subframe 3 [s] - d_TOW_SF4 = 0; //!< Time of GPS Week from HOW word of Subframe 4 [s] - d_TOW_SF5 = 0; //!< Time of GPS Week from HOW word of Subframe 5 [s] + //!< Satellite Identification + i_channel_ID = 0; //!< Channel ID assigned by the receiver + i_satellite_freq_channel = 0; //!< SV Frequency Slot Number + i_satellite_slot_number = 0; //!< SV Orbit Slot Number - d_m = 0.0; //!< String number within frame [dimensionless] - d_t_k = 0.0; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] - d_t_b = 0.0; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] - d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless] - d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] - d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), - d_B_n = 0.0; //!< Health flag [dimensionless] - d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] - d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] - d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] - d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation - d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] - d_E_n = 0.0; //!< Characterises "age" of a current information [days] - d_P_1 = 0.0; //!< Flag of the immediate data updating. - d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] - d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] - d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - d_l_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + //!< Ephmeris Flags + flag_all_ephemeris = false; + flag_ephemeris_str_1 = false; + flag_ephemeris_str_2 = false; + flag_ephemeris_str_3 = false; + flag_ephemeris_str_4 = false; - // Almanac and Not Inmediate Information - d_tau_c = 0.0; //!< GLONASS time scale correction to UTC(SU) time. [s] - d_tau_gps = 0.0; //!< Correction to GPS time to GLONASS time [day] - d_N_4 = 0.0; //!< Four year interval number starting from 1996 [4 year interval] - d_N_A = 0.0; //!< Calendar day number within the four-year period beginning since the leap year [days] - d_n_A = 0.0; //!< Conventional number of satellite within GLONASS space segment [dimensionless] - d_H_n_A = 0.0; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] - d_lambda_n_A = 0.0; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] - d_t_lambda_n_A = 0.0; //!< Time of first ascending node passage [s] - d_Delta_i_n_A = 0.0; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] - d_Delta_T_n_A = 0.0; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period] - d_Delta_T_n_A_dot = 0.0; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2] - d_epsilon_n_A = 0.0; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless] - d_omega_n_A = 0.0; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles] - d_M_n_A = 0.0; //!< Type of satellite n_A [dimensionless] - d_B1 = 0.0; //!< Coefficient to determine DeltaUT1 [s] - d_B2 = 0.0; //!< Coefficient to determine DeltaUT1 [s/msd] - d_KP = 0.0; //!< Notification on forthcoming leap second correction of UTC [dimensionless] - d_tau_n_A = 0.0; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] - d_C_n_A = 0.0; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + //!< Almanac Flags + flag_all_almanac = false; + flag_almanac_str_6 = false; + flag_almanac_str_7 = false; + flag_almanac_str_8 = false; + flag_almanac_str_9 = false; + flag_almanac_str_10 = false; + flag_almanac_str_11 = false; + flag_almanac_str_12 = false; + flag_almanac_str_13 = false; + flag_almanac_str_14 = false; + flag_almanac_str_15 = false; - std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus + //!< UTC and System Clocks Flags + flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled + flag_utc_model_str_5; //!< Clock info send in string 5 of navigation data + flag_utc_model_str_15; //!< Clock info send in string 15 of frame 5 of navigation data + flag_TOW_5; + flag_TOW_6; + flag_TOW_set; //!< it is true when page 5 or page 6 arrives - - /*! \brief If true, enhanced level of integrity assurance. - * - * If false, indicates that the conveying signal is provided with the legacy level of integrity assurance. - * That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound - * value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less - * than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of - * integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73 - * times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an - * accompanying alert, is less than 1E-8 per hour. - */ - b_integrity_status_flag = false; - b_alert_flag = false; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk. - b_antispoofing_flag = false; //!< If true, the AntiSpoofing mode is ON in that SV + //broadcast orbit 1 + //TODO Need to send the information regarding the frame number + double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] + double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s] + double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s] + double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s] + double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s] // Clock terms - d_satClkCorr = 0.0; // Satellite clock error - d_dtr = 0.0; // Relativistic clock correction term - d_satClkDrift = 0.0; // Satellite clock drift + d_satClkCorr = 0.0; + d_dtr = 0.0; + d_satClkDrift = 0.0; - // satellite identification info - int i_channel_ID = 0; - int i_satellite_freq_channel = 0; //!< SV PRN NUMBER - // time synchro - d_subframe_timestamp_ms = 0; //[ms] - - // UTC parameters - bool flag_utc_model_valid = false; //!< If set, it indicates that the UTC model parameters are filled - - // satellite positions - d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity - d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration - d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus auto gnss_sat = Gnss_Satellite(); std::string _system ("GLONASS"); @@ -145,79 +102,97 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() reset(); } + bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data_bits, std::bitset hamming_code_bits ) { - int sum; + int sum_bits; + int sum_hamming; //!< Compute C1 term - sum = 0; + sum_bits = 0; for(int i = 0; i < GLONASS_GNAV_CRC_I_INDEX.size; i++) { - sum += data_bits[GLONASS_GNAV_CRC_I_INDEX[i]] + sum_bits += data_bits[GLONASS_GNAV_CRC_I_INDEX[i]]; } - C1 = hamming_code_bits[0]^fmod(sum,2); + C1 = hamming_code_bits[0]^fmod(sum_bits,2); //!< Compute C2 term - sum = 0; + sum_bits = 0; for(int j = 0; j < GLONASS_GNAV_CRC_J_INDEX.size; j++) { - sum += data_bits[GLONASS_GNAV_CRC_J_INDEX[j]] + sum_bits += data_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; } - C2 = hamming_code_bits[1]^fmod(sum,2); + C2 = hamming_code_bits[1]^fmod(sum_bits,2); //!< Compute C3 term - sum = 0; + sum_bits = 0; for(int k = 0; k < GLONASS_GNAV_CRC_K_INDEX.size; k++) { - sum += data_bits[GLONASS_GNAV_CRC_K_INDEX[k]] + sum_bits += data_bits[GLONASS_GNAV_CRC_K_INDEX[k]]; } - C3 = hamming_code_bits[2]^fmod(sum,2); + C3 = hamming_code_bits[2]^fmod(sum_bits,2); //!< Compute C4 term - sum = 0; + sum_bits = 0; for(int l = 0; l < GLONASS_GNAV_CRC_L_INDEX.size; l++) { - sum += data_bits[GLONASS_GNAV_CRC_L_INDEX[l]] + sum_bits += data_bits[GLONASS_GNAV_CRC_L_INDEX[l]]; } - C4 = hamming_code_bits[3]^fmod(sum,2); + C4 = hamming_code_bits[3]^fmod(sum_bits,2); //!< Compute C5 term - sum = 0; + sum_bits = 0; for(int m = 0; m < GLONASS_GNAV_CRC_M_INDEX.size; m++) { - sum += data_bits[GLONASS_GNAV_CRC_M_INDEX[m]] + sum_bits += data_bits[GLONASS_GNAV_CRC_M_INDEX[m]]; } - C5 = hamming_code_bits[4]^fmod(sum,2); + C5 = hamming_code_bits[4]^fmod(sum_bits,2); //!< Compute C6 term - sum = 0; + sum_bits = 0; for(int n = 0; n < GLONASS_GNAV_CRC_N_INDEX.size; n++) { - sum += data_bits[GLONASS_GNAV_CRC_N_INDEX[n]] + sum_bits += data_bits[GLONASS_GNAV_CRC_N_INDEX[n]]; } - C6 = hamming_code_bits[5]^fmod(sum,2); + C6 = hamming_code_bits[5]^fmod(sum_bits,2); //!< Compute C7 term - sum = 0; + sum_bits = 0; for(int p = 0; p < GLONASS_GNAV_CRC_P_INDEX.size; p++) { - sum += data_bits[GLONASS_GNAV_CRC_P_INDEX[p]] + sum_bits += data_bits[GLONASS_GNAV_CRC_P_INDEX[p]]; } - C7 = hamming_code_bits[6]^fmod(sum,2); + C7 = hamming_code_bits[6]^fmod(sum_bits,2); - //!< Compute C8 term - sum = 0; + //!< Compute C_Sigma term + sum_bits = 0; + sum_hamming = 0; for(int q = 0; q < GLONASS_GNAV_CRC_Q_INDEX.size; q++) { - sum += data_bits[GLONASS_GNAV_CRC_Q_INDEX[q]] + sum_bits += data_bits[GLONASS_GNAV_CRC_Q_INDEX[q]]; } - C8 = hamming_code_bits[7]^fmod(sum,2); + for(int q = 0; q < 8; q++) + { + sum_hamming += hamming_code_bits[q]; + } + C_Sigma = fmod(sum_bits, 2)^fmod(sum_bits,2); - if isempty(find(C,1)) || (length(find(C(1,1:7))) == 1 && C(1,8) == 1) - status = 1; - else - status = 0; - end + + //!< Verification of the data + // All of the checksums are equal to zero + if((C1 & C2 & C3 & C4 & C5 & C6 & C7 & C_Sigma) == 0 ) + { + return true; + } + // only one of the checksums (C1,...,C7) is equal to zero but C_Sigma = 1 + else if(C_Sigma == 1 && C1+C2+C3+C4+C5+C6+C7 == 6) + { + return true; + } + else + { + return false; + } } @@ -315,6 +290,7 @@ signed long int Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bit return value; } + unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int satellite_slot_number) { unsigned int frame_ID = 0; @@ -346,6 +322,7 @@ unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int sate } } + int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) { int string_ID = 0; @@ -591,8 +568,6 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) } - - double Glonass_Gnav_Navigation_Message::utc_time(const double glonass_time_corrected) const { double t_utc; @@ -602,71 +577,25 @@ double Glonass_Gnav_Navigation_Message::utc_time(const double glonass_time_corre } - Glonass_Gnav_Ephemeris Glonass_Gnav_Navigation_Message::get_ephemeris() { - Glonass_Gnav_Ephemeris ephemeris; - - ephemeris.i_satellite_freq_channel = i_satellite_freq_channel; - ephemeris.d_m = d_m; - ephemeris.d_t_k = d_t_k; - ephemeris.d_t_b = d_t_b; - ephemeris.d_M = d_M; - ephemeris.d_gamma_n = d_gamma_n; - ephemeris.d_tau_n = d_tau_n; - // satellite positions - ephemeris.d_satpos_X = d_satpos_X; - ephemeris.d_satpos_Y = d_satpos_Y; - ephemeris.d_satpos_Z = d_satpos_Z; - // Satellite velocity - ephemeris.d_satvel_X = d_satvel_X; - ephemeris.d_satvel_Y = d_satvel_Y; - ephemeris.d_satvel_Z = d_satvel_Z; - // Satellite acceleration - ephemeris.d_satacc_X = d_satacc_X; - ephemeris.d_satacc_Y = d_satacc_Y; - ephemeris.d_satacc_Z = d_satacc_Z; - ephemeris.d_B_n = d_B_n; - ephemeris.d_P = d_P; - ephemeris.d_N_T = d_N_T; - ephemeris.d_F_T = d_F_T; - ephemeris.d_n = d_n; - ephemeris.d_Delta_tau_n = d_Delta_tau_n; - ephemeris.d_E_n = d_E_n; - ephemeris.d_P_1 = d_P_1; - ephemeris.d_P_2 = d_P_2; - ephemeris.d_P_3 = d_P_3; - ephemeris.d_P_4 = d_P_4; - ephemeris.d_l_n = d_l_n; - - // clock terms derived from ephemeris data - ephemeris.d_satClkDrift = d_satClkDrift; - ephemeris.d_dtr = d_dtr; - - return ephemeris; + return gnav_ephemeris; } Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model() { - Gps_Utc_Model utc_model; - utc_model.valid = flag_utc_model_valid; - // UTC parameters - utc_model.d_A1 = d_A1; - utc_model.d_A0 = d_A0; - utc_model.d_t_OT = d_t_OT; - utc_model.i_WN_T = i_WN_T; - utc_model.d_DeltaT_LS = d_DeltaT_LS; - utc_model.i_WN_LSF = i_WN_LSF; - utc_model.i_DN = i_DN; - utc_model.d_DeltaT_LSF = d_DeltaT_LSF; - // warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue - flag_utc_model_valid = false; - return utc_model; + return gnav_utc_model; } -bool Glonass_Gnav_Navigation_Message::satellite_validation() +Glonass_Gnav_Almanac get_almanac() +{ + return gnav_almanac; +} + + +bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class { bool flag_data_valid = false; b_valid_ephemeris_set_flag = false; @@ -674,7 +603,7 @@ bool Glonass_Gnav_Navigation_Message::satellite_validation() // First Step: // check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception // and check if the data have been filled (!=0) - if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0) + if (d_TOW_F1 != 0 and d_TOW_F2 != 0 and d_TOW_F3 != 0) { if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1) { @@ -682,5 +611,67 @@ bool Glonass_Gnav_Navigation_Message::satellite_validation() b_valid_ephemeris_set_flag = true; } } - return flag_data_valid; + + + if ((flag_ephemeris_str_1 == true) and (flag_ephemeris_str_2 == true) and (flag_ephemeris_str_3 == true) and (flag_ephemeris_str_4 == true) and (flag_iono_and_GST == true)) + { + //if all ephemeris pages have the same IOD, then they belong to the same block + if ((gnav_ephemeris.d_t_b== IOD_nav_2) and (IOD_nav_3 == IOD_nav_4) and (IOD_nav_1 == IOD_nav_3)) + { + std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; + flag_ephemeris_1 = false;// clear the flag + flag_ephemeris_2 = false;// clear the flag + flag_ephemeris_3 = false;// clear the flag + flag_ephemeris_4 = false;// clear the flag + flag_all_ephemeris = true; + IOD_ephemeris = IOD_nav_1; + std::cout << "Batch number: "<< IOD_ephemeris << std::endl; + return true; + } + else + { + return false; + } + } + else + return false; +} + + +bool Glonass_Gnav_Navigation_Message::have_new_utc_model() // Check if we have a new utc data set stored in the galileo navigation class +{ + if (flag_utc_model == true) + { + flag_utc_model = false; // clear the flag + return true; + } + else + return false; +} + + +bool Glonass_Gnav_Navigation_Message::have_new_almanac() //Check if we have a new almanac data set stored in the galileo navigation class +{ + if ((flag_almanac_str_6 == true) and (flag_almanac_str_7 == true) and + (flag_almanac_str_8 == true) and (flag_almanac_str_9 == true) and + (flag_almanac_str_10 == true) and (flag_almanac_str_11 == true) and + (flag_almanac_str_12 == true) and (flag_almanac_str_13 == true) and + (flag_almanac_str_14 == true) and (flag_almanac_str_15 == true)) + { + //All almanac have been received + flag_almanac_str_6 = false; + flag_almanac_str_7 = false; + flag_almanac_str_8 = false; + flag_almanac_str_9 = false; + flag_almanac_str_10 = false; + flag_almanac_str_11 = false; + flag_almanac_str_12 = false; + flag_almanac_str_13 = false; + flag_almanac_str_14 = false; + flag_almanac_str_15 = false; + flag_all_almanac = true; + return true; + } + else + return false; } diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 0098f6d9a..d16f4b651 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -64,78 +64,61 @@ private: unsigned int get_frame_number(unsigned int satellite_slot_number); public: - bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check - int Page_type_time_stamp; - int flag_even_word; - std::string page_Even; bool flag_CRC_test; + unsigned int u_frame_number; Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded - Glonass_Gnav_Iono gnav_iono; //!< Iono corrections information + //Glonass_Gnav_Iono gnav_iono; //!< Iono corrections information Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites + //!< Satellite Identification + int i_channel_ID; //!< Channel ID assigned by the receiver + int i_satellite_freq_channel; //!< SV Frequency Slot Number + int i_satellite_slot_number; //!< SV Orbit Slot Number + //!< Ephmeris Flags bool flag_all_ephemeris; //!< Flag indicating that all strings containing ephemeris have been received - bool flag_ephemeris_str_1; //!< Flag indicating that ephemeris 1/4 (word 1) have been received - bool flag_ephemeris_str_2; //!< Flag indicating that ephemeris 2/4 (word 2) have been received - bool flag_ephemeris_str_3; //!< Flag indicating that ephemeris 3/4 (word 3) have been received - bool flag_ephemeris_str_4; //!< Flag indicating that ephemeris 4/4 (word 4) have been received + bool flag_ephemeris_str_1; //!< Flag indicating that ephemeris 1/4 (string 1) have been received + bool flag_ephemeris_str_2; //!< Flag indicating that ephemeris 2/4 (string 2) have been received + bool flag_ephemeris_str_3; //!< Flag indicating that ephemeris 3/4 (string 3) have been received + bool flag_ephemeris_str_4; //!< Flag indicating that ephemeris 4/4 (string 4) have been received - bool flag_iono_and_GST; //!< Flag indicating that ionospheric and GST parameters (word 5) have been received + //!< Almanac Flags + bool flag_all_almanac; //!< Flag indicating that all almanac have been received + bool flag_almanac_str_6; //!< Flag indicating that almanac of string 6 have been received + bool flag_almanac_str_7; //!< Flag indicating that almanac of string 7 have been received + bool flag_almanac_str_8; //!< Flag indicating that almanac of string 8 have been received + bool flag_almanac_str_9; //!< Flag indicating that almanac of string 9 have been received + bool flag_almanac_str_10; //!< Flag indicating that almanac of string 10 have been received + bool flag_almanac_str_11; //!< Flag indicating that almanac of string 11 have been received + bool flag_almanac_str_12; //!< Flag indicating that almanac of string 12 have been received + bool flag_almanac_str_13; //!< Flag indicating that almanac of string 13 have been received + bool flag_almanac_str_14; //!< Flag indicating that almanac of string 14 have been received + bool flag_almanac_str_15; //!< Flag indicating that almanac of string 15 have been received + + //!< UTC and System Clocks Flags + bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled + bool flag_utc_model_str_5; //!< Clock info send in string 5 of navigation data + bool flag_utc_model_str_15; //!< Clock info send in string 15 of frame 5 of navigation data bool flag_TOW_5; bool flag_TOW_6; - bool flag_TOW_set; //!< it is true when page 5 or page 6 arrives - bool flag_utc_model; //!< Flag indicating that utc model parameters (word 6) have been received - - bool flag_all_almanac; //!< Flag indicating that all almanac have been received - bool flag_almanac_str_6; //!< Flag indicating that almanac 1/4 (word 7) have been received - bool flag_almanac_str_7; //!< Flag indicating that almanac 2/4 (word 8) have been received - bool flag_almanac_str_8; //!< Flag indicating that almanac 3/4 (word 9) have been received - bool flag_almanac_str_9; //!< Flag indicating that almanac 4/4 (word 10) have been received - bool flag_almanac_str_10; //!< Flag indicating that almanac 4/4 (word 10) have been received - bool flag_almanac_str_11; //!< Flag indicating that almanac 4/4 (word 10) have been received - bool flag_almanac_str_12; //!< Flag indicating that almanac 4/4 (word 10) have been received - bool flag_almanac_str_13; //!< Flag indicating that almanac 4/4 (word 10) have been received - bool flag_almanac_str_14; //!< Flag indicating that almanac 4/4 (word 10) have been received - bool flag_almanac_str_15; //!< Flag indicating that almanac 4/4 (word 10) have been received + bool flag_TOW_set; //!< it is true when page 5 or page 6 arrives //broadcast orbit 1 //TODO Need to send the information regarding the frame number - double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] - double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s] - double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s] - double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s] - double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s] + double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] + double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s] + double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s] + double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s] + double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s] // Clock terms double d_satClkCorr; // Satellite clock error double d_dtr; // Relativistic clock correction term double d_satClkDrift; // Satellite clock drift - // satellite identification info - int i_channel_ID; - unsigned int i_satellite_PRN; //!< SV PRN Number - int i_satellite_freq_channel; //!< SV Frequency Slot Number - int i_satellite_slot_number; //!< SV Orbit Slot Number - - // UTC parameters - bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled - - // satellite positions - double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity - double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration - double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] - /*! * \brief Reset GLONASS GNAV Navigation Information */ @@ -182,12 +165,10 @@ public: */ bool have_new_almanac(); - - /*! - * \brief Decodes the GLONASS GNAV frame + * \brief Decodes the GLONASS GNAV string */ - int string_decoder(char *string, int frame_ID); + int string_decoder(char *string); /*! * \brief Computes the Coordinated Universal Time (UTC) and returns it in [s] From 50f133e960bd353ec01bdae7b1b61f8053e6e01a Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 12 Jul 2017 07:35:57 -0700 Subject: [PATCH 23/48] Fixing compilation warnings --- src/core/system_parameters/GLONASS_L1_CA.h | 3 +- .../glonass_gnav_navigation_message.cc | 77 +++++++++++-------- .../glonass_gnav_navigation_message.h | 17 +--- 3 files changed, 47 insertions(+), 50 deletions(-) diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 158dae798..7e6699b8f 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -104,10 +104,11 @@ const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame i const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] const int GLONASS_GNAV_FRAME_MS = 30000; //!< Subframe duration [seconds] const int GLONASS_GNAV_STRING_BITS = 115; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] +const int GLONASS_GNAV_HAMMING_CODE_BITS = 115; //!< Number of bits in hamming code sequence of GNAV message const std::vector GLONASS_GNAV_CRC_I_INDEX {9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84}; const std::vector GLONASS_GNAV_CRC_J_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; -const std::vector GLONASS_GNAV_CRC_K_INDEX {10, 11, 12, 16, 17, 18, 19, 23, 24, 25, 26, 31, 32, 33, 34 38, 39, 40, 41, 46, 47, 48, 49, 54, 55, 56, 57, 62, 63, 64, 65, 69, 70, 71, 72, 77, 78, 79, 80, 85}; +const std::vector GLONASS_GNAV_CRC_K_INDEX {10, 11, 12, 16, 17, 18, 19, 23, 24, 25, 26, 31, 32, 33, 34, 38, 39, 40, 41, 46, 47, 48, 49, 54, 55, 56, 57, 62, 63, 64, 65, 69, 70, 71, 72, 77, 78, 79, 80, 85}; const std::vector GLONASS_GNAV_CRC_L_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; const std::vector GLONASS_GNAV_CRC_M_INDEX {20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 81, 82, 83, 84, 85}; const std::vector GLONASS_GNAV_CRC_N_INDEX {35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85}; diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index db5b84eee..5abd0351b 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -63,21 +63,21 @@ void Glonass_Gnav_Navigation_Message::reset() flag_almanac_str_15 = false; //!< UTC and System Clocks Flags - flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled - flag_utc_model_str_5; //!< Clock info send in string 5 of navigation data - flag_utc_model_str_15; //!< Clock info send in string 15 of frame 5 of navigation data - flag_TOW_5; - flag_TOW_6; - flag_TOW_set; //!< it is true when page 5 or page 6 arrives + flag_utc_model_valid = false; //!< If set, it indicates that the UTC model parameters are filled + flag_utc_model_str_5 = false; //!< Clock info send in string 5 of navigation data + flag_utc_model_str_15 = false; //!< Clock info send in string 15 of frame 5 of navigation data + flag_TOW_5 = false; + flag_TOW_6 = false; + flag_TOW_set = false; //!< it is true when page 5 or page 6 arrives //broadcast orbit 1 //TODO Need to send the information regarding the frame number - double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] - double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s] - double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s] - double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s] - double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s] + d_TOW = false; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + d_TOW_F1 = false; //!< Time of GPS Week from HOW word of Subframe 1 [s] + d_TOW_F2 = false; //!< Time of GPS Week from HOW word of Subframe 2 [s] + d_TOW_F3 = false; //!< Time of GPS Week from HOW word of Subframe 3 [s] + d_TOW_F4 = false; //!< Time of GPS Week from HOW word of Subframe 4 [s] + d_TOW_F5 = false; //!< Time of GPS Week from HOW word of Subframe 5 [s] // Clock terms d_satClkCorr = 0.0; @@ -103,30 +103,37 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() } -bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data_bits, std::bitset hamming_code_bits ) +bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data_bits, std::bitset hamming_code_bits ) { int sum_bits; int sum_hamming; - + int C1 = 0; + int C2 = 0; + int C3 = 0; + int C4 = 0; + int C5 = 0; + int C6 = 0; + int C7 = 0; + int C_Sigma = 0; //!< Compute C1 term sum_bits = 0; - for(int i = 0; i < GLONASS_GNAV_CRC_I_INDEX.size; i++) + for(int i = 0; i < GLONASS_GNAV_CRC_I_INDEX.size(); i++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_I_INDEX[i]]; + sum_bits += static_cast(data_bits[GLONASS_GNAV_CRC_I_INDEX[i]]); } - C1 = hamming_code_bits[0]^fmod(sum_bits,2); + C1 = static_cast(hamming_code_bits[0])^static_cast(fmod(sum_bits,2)); //!< Compute C2 term sum_bits = 0; - for(int j = 0; j < GLONASS_GNAV_CRC_J_INDEX.size; j++) + for(int j = 0; j < GLONASS_GNAV_CRC_J_INDEX.size(); j++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; + sum_bits += static_castdata_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; } - C2 = hamming_code_bits[1]^fmod(sum_bits,2); + C2 = static_cast(hamming_code_bits[1])^static_cast(fmod(sum_bits,2)); //!< Compute C3 term sum_bits = 0; - for(int k = 0; k < GLONASS_GNAV_CRC_K_INDEX.size; k++) + for(int k = 0; k < GLONASS_GNAV_CRC_K_INDEX.size(); k++) { sum_bits += data_bits[GLONASS_GNAV_CRC_K_INDEX[k]]; } @@ -134,7 +141,7 @@ bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset(read_navigation_unsigned(string_bits, OMEGA_N_A)); @@ -613,16 +622,16 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a } - if ((flag_ephemeris_str_1 == true) and (flag_ephemeris_str_2 == true) and (flag_ephemeris_str_3 == true) and (flag_ephemeris_str_4 == true) and (flag_iono_and_GST == true)) + if ((flag_ephemeris_str_1 == true) and (flag_ephemeris_str_2 == true) and (flag_ephemeris_str_3 == true) and (flag_ephemeris_str_4 == true)) { //if all ephemeris pages have the same IOD, then they belong to the same block - if ((gnav_ephemeris.d_t_b== IOD_nav_2) and (IOD_nav_3 == IOD_nav_4) and (IOD_nav_1 == IOD_nav_3)) + if ((gnav_ephemeris.d_t_b == IOD_nav_2) and (IOD_nav_3 == IOD_nav_4) and (IOD_nav_1 == IOD_nav_3)) { std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; - flag_ephemeris_1 = false;// clear the flag - flag_ephemeris_2 = false;// clear the flag - flag_ephemeris_3 = false;// clear the flag - flag_ephemeris_4 = false;// clear the flag + flag_ephemeris_str_1 = false;// clear the flag + flag_ephemeris_str_2 = false;// clear the flag + flag_ephemeris_str_3 = false;// clear the flag + flag_ephemeris_str_4 = false;// clear the flag flag_all_ephemeris = true; IOD_ephemeris = IOD_nav_1; std::cout << "Batch number: "<< IOD_ephemeris << std::endl; diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index d16f4b651..ebcb99156 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -41,7 +41,6 @@ #include #include "GLONASS_L1_CA.h" #include "glonass_gnav_ephemeris.h" -//#include "gps_iono.h" #include "glonass_gnav_almanac.h" #include "glonass_gnav_utc_model.h" @@ -59,16 +58,15 @@ private: unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); bool read_navigation_bool(std::bitset bits, const std::vector> parameter); - bool _CRC_test(std::bitset bits,boost::uint32_t checksum); + bool _CRC_test(std::bitset bits, std::bitset hamming_code); unsigned int get_frame_number(unsigned int satellite_slot_number); public: bool flag_CRC_test; - unsigned int u_frame_number; + unsigned int frame_number; Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded - //Glonass_Gnav_Iono gnav_iono; //!< Iono corrections information Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites @@ -129,12 +127,6 @@ public: */ Glonass_Gnav_Ephemeris get_ephemeris(); - /*! - // TODO Should I keep this function ? - * \brief Obtain a GPS ionospheric correction parameters class filled with current SV data - */ - Glonass_Gnav_Iono get_iono(); - /*! * \brief Obtain a GLONASS GNAV UTC model parameters class filled with current SV data */ @@ -150,11 +142,6 @@ public: */ bool have_new_ephemeris(); - /* - * \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed - */ - bool have_new_iono(); - /* * \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed */ From 8a0689fbb171ece0e577d8a8477814766cbc8218 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 12 Jul 2017 13:11:53 -0700 Subject: [PATCH 24/48] Fixing compile warnings and adding work on unit test --- src/core/system_parameters/GLONASS_L1_CA.h | 5 +- .../system_parameters/glonass_gnav_almanac.cc | 2 +- .../system_parameters/glonass_gnav_almanac.h | 4 +- .../glonass_gnav_navigation_message.cc | 138 +++++++++--------- .../glonass_gnav_navigation_message.h | 4 +- src/tests/test_main.cc | 1 - .../glonass_gnav_almanac_test.cc | 8 +- 7 files changed, 84 insertions(+), 78 deletions(-) diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 7e6699b8f..33b21b18f 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -178,7 +178,10 @@ const std::vector> T_LAMBDA_N_A({{22,21}}); const std::vector> DELTA_T_N_A({{43,22}}); const std::vector> DELTA_T_DOT_N_A({{65,7}}); const std::vector> H_N_A({{72,5}}); -//const std::vector> L_N({{77,1}}); + +// STRING 14 FRAME 5 +const std::vector> B1({{6,11}}); +const std::vector> B2({{17,10}}); #endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index ae1f492c7..cdde02086 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -52,7 +52,7 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() d_M_n_A = 0.0; d_KP = 0.0; d_tau_n_A = 0.0; - d_C_n_A = 0.0; + d_C_n = 0.0; } void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index f4ce97312..7bbf9fe41 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -62,7 +62,7 @@ public: double d_M_n_A; //!< Type of satellite n_A [dimensionless] double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless] double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] - double d_C_n_A; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + double d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] // satellite positions double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. @@ -108,7 +108,7 @@ public: archive & make_nvp("d_M_n_A", d_M_n_A); archive & make_nvp("d_KP", d_KP); archive & make_nvp("d_tau_n_A", d_tau_n_A); - archive & make_nvp("d_C_n_A", d_C_n_A); + archive & make_nvp("d_C_n", d_C_n); } void satellite_position(double N_i, double t_i); diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 5abd0351b..d7c059d7e 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -32,6 +32,7 @@ m * \file glonass_gnav_navigation_message.cc #include "glonass_gnav_navigation_message.h" #include #include +#include #include @@ -103,7 +104,7 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() } -bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data_bits, std::bitset hamming_code_bits ) +bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset bits) { int sum_bits; int sum_hamming; @@ -115,66 +116,75 @@ bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data = std::bitset(bits.to_string(), 0, 77); + std::bitset hamming_code = std::bitset(bits.to_string(), 77, 8); + + std::istringstream dsb = std::istringstream( data.to_string() ); + std::istringstream hcb = std::istringstream( hamming_code.to_string() ); + std::vector data_bits = std::vector( std::istream_iterator( dsb ), std::istream_iterator() ); + std::vector hamming_code_bits = std::vector( std::istream_iterator( dsb ), std::istream_iterator() ); + //!< Compute C1 term sum_bits = 0; - for(int i = 0; i < GLONASS_GNAV_CRC_I_INDEX.size(); i++) + for(int i = 0; i < static_cast(GLONASS_GNAV_CRC_I_INDEX.size()); i++) { - sum_bits += static_cast(data_bits[GLONASS_GNAV_CRC_I_INDEX[i]]); + sum_bits += data_bits[GLONASS_GNAV_CRC_I_INDEX[i]]; } - C1 = static_cast(hamming_code_bits[0])^static_cast(fmod(sum_bits,2)); + C1 = hamming_code_bits[0]^(sum_bits%2); //!< Compute C2 term sum_bits = 0; - for(int j = 0; j < GLONASS_GNAV_CRC_J_INDEX.size(); j++) + for(int j = 0; j < static_cast(GLONASS_GNAV_CRC_J_INDEX.size()); j++) { - sum_bits += static_castdata_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; + sum_bits += data_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; } - C2 = static_cast(hamming_code_bits[1])^static_cast(fmod(sum_bits,2)); + C2 = (hamming_code_bits[1])^(sum_bits%2); //!< Compute C3 term sum_bits = 0; - for(int k = 0; k < GLONASS_GNAV_CRC_K_INDEX.size(); k++) + for(int k = 0; k < static_cast(GLONASS_GNAV_CRC_K_INDEX.size()); k++) { sum_bits += data_bits[GLONASS_GNAV_CRC_K_INDEX[k]]; } - C3 = hamming_code_bits[2]^fmod(sum_bits,2); + C3 = hamming_code_bits[2]^(sum_bits%2); //!< Compute C4 term sum_bits = 0; - for(int l = 0; l < GLONASS_GNAV_CRC_L_INDEX.size(); l++) + for(int l = 0; l < static_cast(GLONASS_GNAV_CRC_L_INDEX.size()); l++) { sum_bits += data_bits[GLONASS_GNAV_CRC_L_INDEX[l]]; } - C4 = hamming_code_bits[3]^fmod(sum_bits,2); + C4 = hamming_code_bits[3]^(sum_bits%2); //!< Compute C5 term sum_bits = 0; - for(int m = 0; m < GLONASS_GNAV_CRC_M_INDEX.size(); m++) + for(int m = 0; m < static_cast(GLONASS_GNAV_CRC_M_INDEX.size()); m++) { sum_bits += data_bits[GLONASS_GNAV_CRC_M_INDEX[m]]; } - C5 = hamming_code_bits[4]^fmod(sum_bits,2); + C5 = hamming_code_bits[4]^(sum_bits%2); //!< Compute C6 term sum_bits = 0; - for(int n = 0; n < GLONASS_GNAV_CRC_N_INDEX.size(); n++) + for(int n = 0; n < static_cast(GLONASS_GNAV_CRC_N_INDEX.size()); n++) { sum_bits += data_bits[GLONASS_GNAV_CRC_N_INDEX[n]]; } - C6 = hamming_code_bits[5]^fmod(sum_bits,2); + C6 = hamming_code_bits[5]^(sum_bits%2); //!< Compute C7 term sum_bits = 0; - for(int p = 0; p < GLONASS_GNAV_CRC_P_INDEX.size(); p++) + for(int p = 0; p < static_cast(GLONASS_GNAV_CRC_P_INDEX.size()); p++) { sum_bits += data_bits[GLONASS_GNAV_CRC_P_INDEX[p]]; } - C7 = hamming_code_bits[6]^fmod(sum_bits,2); + C7 = hamming_code_bits[6]^(sum_bits%2); //!< Compute C_Sigma term sum_bits = 0; sum_hamming = 0; - for(int q = 0; q < GLONASS_GNAV_CRC_Q_INDEX.size(); q++) + for(int q = 0; q < static_cast(GLONASS_GNAV_CRC_Q_INDEX.size()); q++) { sum_bits += data_bits[GLONASS_GNAV_CRC_Q_INDEX[q]]; } @@ -182,7 +192,7 @@ bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset string_bits(std::string(frame_string)); - std::bitset data_bits(std::string(frame_string), 0, 77); - std::bitset hamming_code_bits(std::string(frame_string), 77, 8); - + std::bitset string_bits = std::bitset(std::string(frame_string)); string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); + _CRC_test(string_bits); + // Decode all 15 string messages switch (string_ID) { @@ -355,6 +364,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)); gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)); + flag_ephemeris_str_1 = true; + break; case 2: @@ -366,6 +377,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)); gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)); + flag_ephemeris_str_2 = true; + break; case 3: @@ -378,6 +391,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)); gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)); + flag_ephemeris_str_3 = true; + break; case 4: @@ -392,25 +407,27 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_n = static_cast(read_navigation_unsigned(string_bits, N)); gnav_ephemeris.d_M = static_cast(read_navigation_unsigned(string_bits, M)); + flag_ephemeris_str_4 = true; + break; case 5: // --- It is string 5 ---------------------------------------------- // TODO signed vs unsigned reading from datasheet - d_N_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); - d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); - d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); - d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_utc_model.d_N_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_utc_model.d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + gnav_utc_model.d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); + gnav_utc_model.d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); break; case 6: // --- It is string 6 ---------------------------------------------- // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); @@ -432,7 +449,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_7 = true; } @@ -442,8 +459,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 8: // --- It is string 8 ---------------------------------------------- // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); @@ -465,15 +482,15 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_9 = true; } case 10: // --- It is string 8 ---------------------------------------------- // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); @@ -495,7 +512,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_11 = true; } @@ -503,8 +520,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 12: // --- It is string 8 ---------------------------------------------- // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); @@ -526,7 +543,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_13 = true; } @@ -534,14 +551,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // --- It is string 8 ---------------------------------------------- if( frame_number == 5) { - gnav_utc_model.B1 = static_cast(read_navigation_unsigned(string_bits, B1)); - gnav_utc_model.B2 = static_cast(read_navigation_unsigned(string_bits, B2)); + gnav_utc_model.d_B1 = static_cast(read_navigation_unsigned(string_bits, B1)); + gnav_utc_model.d_B2 = static_cast(read_navigation_unsigned(string_bits, B2)); } else { // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); @@ -565,7 +582,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_15 = true; } @@ -573,7 +590,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) break; } // switch subframeID ... - return subframe_ID; + return frame_ID; } @@ -581,7 +598,7 @@ double Glonass_Gnav_Navigation_Message::utc_time(const double glonass_time_corre { double t_utc; - t_utc = glonass_time_corrected + 3*3600 + d_tau_c; + t_utc = glonass_time_corrected + 3*3600 + gnav_utc_model.d_tau_c; return t_utc; } @@ -598,43 +615,29 @@ Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model() } -Glonass_Gnav_Almanac get_almanac() +Glonass_Gnav_Almanac Glonass_Gnav_Navigation_Message::get_almanac( int satellite_slot_number) { - return gnav_almanac; + return gnav_almanac[satellite_slot_number - 1]; } bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class { bool flag_data_valid = false; - b_valid_ephemeris_set_flag = false; - - // First Step: - // check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception - // and check if the data have been filled (!=0) - if (d_TOW_F1 != 0 and d_TOW_F2 != 0 and d_TOW_F3 != 0) - { - if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1) - { - flag_data_valid = true; - b_valid_ephemeris_set_flag = true; - } - } - + bool b_valid_ephemeris_set_flag = false; if ((flag_ephemeris_str_1 == true) and (flag_ephemeris_str_2 == true) and (flag_ephemeris_str_3 == true) and (flag_ephemeris_str_4 == true)) { //if all ephemeris pages have the same IOD, then they belong to the same block - if ((gnav_ephemeris.d_t_b == IOD_nav_2) and (IOD_nav_3 == IOD_nav_4) and (IOD_nav_1 == IOD_nav_3)) + if ((gnav_ephemeris.d_t_b == 0) ) { - std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; flag_ephemeris_str_1 = false;// clear the flag flag_ephemeris_str_2 = false;// clear the flag flag_ephemeris_str_3 = false;// clear the flag flag_ephemeris_str_4 = false;// clear the flag flag_all_ephemeris = true; - IOD_ephemeris = IOD_nav_1; - std::cout << "Batch number: "<< IOD_ephemeris << std::endl; + // std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; + // std::cout << "Batch number: "<< IOD_ephemeris << std::endl; return true; } else @@ -649,6 +652,7 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a bool Glonass_Gnav_Navigation_Message::have_new_utc_model() // Check if we have a new utc data set stored in the galileo navigation class { + bool flag_utc_model = true; if (flag_utc_model == true) { flag_utc_model = false; // clear the flag diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index ebcb99156..18d9025f2 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -58,7 +58,7 @@ private: unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); bool read_navigation_bool(std::bitset bits, const std::vector> parameter); - bool _CRC_test(std::bitset bits, std::bitset hamming_code); + bool _CRC_test(std::bitset bits); unsigned int get_frame_number(unsigned int satellite_slot_number); @@ -135,7 +135,7 @@ public: /* * \brief Returns a Galileo_Almanac object filled with the latest navigation data received */ - Glonass_Gnav_Almanac get_almanac(); + Glonass_Gnav_Almanac get_almanac(int satellite_slot_number); /* * \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 36c5620d7..2f4df5220 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -61,7 +61,6 @@ #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_almanac.h" -#include "glonass_gnav_iono.h" #include "glonass_gnav_utc_model.h" using google::LogMessage; diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc index 650daa341..6c02311cf 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc @@ -43,18 +43,18 @@ #include "glonass_gnav_almanac.h" // See A 3.2.3 -TEST(GlonassGnavEphemerisTest, SatellitePosition) +TEST(GlonassGnavAlmanacTest, SatellitePosition) { double N_i = 615; // [days] - double t_i = 33300.0 // [seconds] + double t_i = 33300.0; // [seconds] double Xoi = 10947.021572; // [km] double Yoi = 13078.978287; // [km] double Zoi = 18922.063362; // [km] - double Vxoi = -3.375497 // [m/s] + double Vxoi = -3.375497; // [m/s] double Vyoi = -0.161453; // [Кm/s] double Vzoi = 2.060844; // [Кm/s] - Glonass_Gnav_Ephemeris gnav_almanac; + Glonass_Gnav_Almanac gnav_almanac; gnav_almanac.d_N_A = 615; // [days] gnav_almanac.d_lambda_n_A = -0.189986229; // [half cycles] From fbd6b211eb01d771fe91600603ec084ed7424627 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Mon, 17 Jul 2017 20:09:10 -0700 Subject: [PATCH 25/48] Adding code for pvt computation --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 291 +++++++++++++++++- src/algorithms/PVT/libs/nmea_printer.cc | 3 - src/algorithms/PVT/libs/rtklib_solver.cc | 77 ++++- src/algorithms/PVT/libs/rtklib_solver.h | 13 + .../glonass_gnav_navigation_message.cc | 11 +- src/core/system_parameters/gnss_satellite.cc | 7 +- 6 files changed, 386 insertions(+), 16 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 152fc6f1d..6ef870b65 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -180,7 +180,37 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) d_ls_pvt->galileo_almanac = *galileo_almanac; DLOG(INFO) << "New Galileo Almanac has arrived "; } - + //**************** GLONASS GNAV Telemetry ************************** + else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GLONASS GNAV EPHEMERIS ### + std::shared_ptr glonass_gnav_eph; + glonass_gnav_eph = boost::any_cast>(pmt::any_ref(msg)); + // TODO Add GLONASS with gps week number and tow, + // insert new ephemeris record + DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->TOW_5 + << ", GLONASS GNAV Week Number =" << glonass_gnav_eph->WN_5 + << " and Ephemeris IOD = " << glonass_gnav_eph->IOD_ephemeris; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; + } + else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GLONASS GNAV UTC MODEL ### + std::shared_ptr glonass_gnav_utc_model; + glonass_gnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->glonass_gnav_utc_model = *glonass_gnav_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GLONASS GNAV Almanac ### + std::shared_ptr glonass_gnav_almanac; + glonass_gnav_almanac = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->glonass_gnav_almanac = *glonass_gnav_almanac; + DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "; + } else { LOG(WARNING) << "msg_handler_telemetry unknown object type!"; @@ -403,6 +433,28 @@ rtklib_pvt_cc::~rtklib_pvt_cc() LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; } + //save GLONASS GNAV ephemeris to XML file + file_name = "eph_GLONASS_GNAV.xml"; + + if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->glonass_gnav_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GLONASS GNAV Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GLONASS GNAV Ephemeris, map is empty"; + } if (d_dump_file.is_open() == true) { try @@ -455,6 +507,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite bool flag_write_RINEX_nav_output = false; unsigned int gps_channel = 0; unsigned int gal_channel = 0; + unsigned int glo_channel = 0; gnss_observables_map.clear(); const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer @@ -467,10 +520,13 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite std::map::const_iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); + std::map::const_iterator tmp_eph_iter_glo_gnav = d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); if(((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) - || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0))) + || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0)) + || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) + || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2C") == 0))) { // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); @@ -497,6 +553,14 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } } + if(d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) + { + if(tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + } } @@ -613,6 +677,15 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite * 20 | GPS L5 + Galileo E5b * 21 | GPS L1 C/A + Galileo E1B + GPS L2C * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + * 23 | GLONASS L1 C/A + * 24 | GLONASS L2 C/A + * 25 | GLONASS L1 C/A + GLONASS L2 C/A + * 26 | GLONASS L1 C/A + GPS L1 C/A + * 27 | GLONASS L1 C/A + GPS L2C + * 28 | GLONASS L1 C/A + GPS L5 + * 29 | GLONASS L1 C/A + Galileo E1B + * 30 | GLONASS L1 C/A + Galileo E5a + * 31 | GLONASS L1 C/A + Galileo E5b */ // ####################### RINEX FILES ################# @@ -620,6 +693,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; + std::map::iterator glonass_gnav_ephemeris_iter; std::map::const_iterator gnss_observables_iter; if (!b_rinex_header_written) // & we have utc data in nav message! @@ -627,6 +701,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); if(type_of_rx == 1) // GPS L1 C/A only { @@ -736,6 +811,87 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_written = true; // do not write header anymore } } + if(type_of_rx == 23) // GLONASS L1 C/A only + { + std::string signal("1C"); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 24) // GLONASS L2 C/A only + { + std::string signal("2C"); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 25) // GLONASS L1 C/A + GLONASS L2 C/A + { + std::string signal("1C 2C"); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + + if(type_of_rx == 26) // GLONASS L1 C/A + GPS L1 C/A + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) ) + { + std::string gps_signal("1C 1C"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 27) // GLONASS L1 C/A + GPS L2C + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) ) + { + std::string gps_signal("1C 1C"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 29) // GLONASS L1 C/A + Galileo E1B + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) + { + std::string gal_signal("1C 1B"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 30) // GLONASS L1 C/A + Galileo E5a + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) + { + std::string gal_signal("1C 5X"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 31) // GLONASS L1 C/A + Galileo E5b + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) + { + std::string gal_signal("1C 7X"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } } if(b_rinex_header_written) // The header is already written, we can now log the navigation message data { @@ -765,10 +921,27 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } + if((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS L1 C/A, GLONASS L2 C/A + { + rp->log_rinex_nav(rp->navGloFile, d_ls_pvt->glonass_gnav_ephemeris_map); + } + if((type_of_rx == 26)) // GLONASS L1 C/A + GPS L1 C/A + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->gps_ephemeris_map); + } + if((type_of_rx == 27)) // GLONASS L1 C/A + GPS L2C + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->gps_cnav_ephemeris_map); + } + if((type_of_rx == 29) || (type_of_rx == 30) || (type_of_rx == 31)) // GLONASS L1 C/A + Galileo + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } } galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); // Log observables into the RINEX file if(flag_write_RINEX_obs_output) @@ -890,6 +1063,120 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_updated = true; } } + if(type_of_rx == 23) // GLONASS L1 C/A only + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C"); + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 24) // GLONASS L2 C/A only + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "2C"); + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 25) // GLONASS L1 C/A + GLONASS L2 C/A + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C 2C"); + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 26) // GLONASS L1 C/A + GPS L1 C/A + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gps_signal("1C 1C"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 27) // GLONASS L1 C/A + GPS L1 C/A + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) + { + std::string gps_signal("1C 2S"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gps_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 29) // GLONASS L1 C/A + Galileo E1B + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + { + std::string gal_signal("1C 1B"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 30) // GLONASS L1 C/A + Galileo E5a + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + { + std::string gal_signal("1C 5X"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 31) // GLONASS L1 C/A + Galileo E5b + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + { + std::string gal_signal("1C 7X"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; // do not write header anymore + } + } } } diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index 97ab793a7..b7d55d67f 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -727,6 +727,3 @@ std::string Nmea_Printer::get_GPGGA() return sentence_str.str(); //$GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A } - - - diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 344888f67..b46fe7576 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -56,6 +56,7 @@ #include "rtklib_conversions.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" +#include "GLONASS_L1_CA.h" using google::LogMessage; @@ -114,6 +115,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; + std::map::const_iterator glonass_gnav_ephemeris_iter; this->set_averaging_flag(flag_averaging); @@ -131,7 +133,9 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ { switch(gnss_observables_iter->second.System) { - case 'E': + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) { std::string sig_(gnss_observables_iter->second.Signal); // Galileo E1 @@ -270,6 +274,76 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } break; } + case 'R': //TODO This should be using rtk lib nomenclature + { + std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if(sig_.compare("1C") == 0) + { + // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.WN_5, + 0);//TODO are THESE VALUES OK + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + + } + // GLONASS GNAV L2 + if(sig_.compare("2C") == 0) + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) + { + bool found_L1_obs=false; + for (int i = 0; i < valid_obs; i++) + { + // TODO what is this? + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN+NSATGPS+NSATGLO))) + { + obs_data[i] = insert_obs_to_rtklib(obs_data[i], + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.WN_5, + 2);//Band 3 (L5/E5) + found_L1_obs=true; + break; + } + } + if (!found_L1_obs) + { + //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 2); //Band 3 (L5/E5) + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + + + } + break; + } default : DLOG(INFO) << "Hybrid observables: Unknown GNSS"; break; @@ -365,6 +439,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } } } + } } return this->is_valid_position(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 6f441f4a6..274289138 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -62,6 +62,7 @@ #include "galileo_navigation_message.h" #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" +#include "glonass_gnav_navigation_message.h" #include "gnss_synchro.h" #include "pvt_solution.h" @@ -86,6 +87,7 @@ public: std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris std::map gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris + std::map glonass_gnav_ephemeris_map; //!< Map storing new GLONASS GNAV Ephmeris Galileo_Utc_Model galileo_utc_model; Galileo_Iono galileo_iono; @@ -97,7 +99,18 @@ public: Gps_CNAV_Iono gps_cnav_iono; Gps_CNAV_Utc_Model gps_cnav_utc_model; + Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< GLONASS GNAV UTC Model + Glonass_Gnav_Almanac glonass_gnav_almanac; //!< GLONASS GNAV Almanac Model + int count_valid_position; + + bool d_flag_dump_enabled; + + sol_t pvt_sol; + rtk_t rtk_; + + std::string d_dump_filename; + std::ofstream d_dump_file; }; #endif diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index d7c059d7e..96608b736 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -449,7 +449,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_7 = true; } @@ -482,7 +482,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_9 = true; } @@ -512,7 +512,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_11 = true; } @@ -543,7 +543,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_13 = true; } @@ -582,7 +582,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_15 = true; } @@ -631,6 +631,7 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a //if all ephemeris pages have the same IOD, then they belong to the same block if ((gnav_ephemeris.d_t_b == 0) ) { + std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; flag_ephemeris_str_1 = false;// clear the flag flag_ephemeris_str_2 = false;// clear the flag flag_ephemeris_str_3 = false;// clear the flag diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index b192224fb..6e0a6e4d8 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -157,7 +157,7 @@ void Gnss_Satellite::set_PRN(unsigned int PRN_) PRN = PRN_; } } - else if (system.compare("Glonass") == 0) + else if (system.compare("GLONASS") == 0) { if (PRN_ < 1 or PRN_ > 24) { @@ -356,7 +356,7 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int } - if (system_.compare("Glonass") == 0) + if (system_.compare("GLONASS") == 0) { switch ( PRN_ ) { @@ -557,6 +557,3 @@ void Gnss_Satellite::set_block(const std::string& system_, unsigned int PRN_) { block = what_block(system_, PRN_); } - - - From c445ac261b0e8f547a530720cfed15482d769400 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Mon, 17 Jul 2017 21:50:36 -0700 Subject: [PATCH 26/48] Developing interface code for RINEX printer --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 1 + src/algorithms/PVT/libs/rinex_printer.h | 79 +++++++++++++++++++ 2 files changed, 80 insertions(+) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 6ef870b65..d3fa0af34 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1181,6 +1181,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } // ####################### RTCM MESSAGES ################# + //TODO Add RTCM logic for GLONASS satellites if(b_rtcm_writing_started) { if(type_of_rx == 1) // GPS L1 C/A diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index d9aee967e..c4029f868 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -61,8 +61,10 @@ #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" #include "galileo_navigation_message.h" +#include "glonass_gnav_navigation_message.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" +#include "GLONASS_L1_CA.h" #include "gnss_synchro.h" class Sbas_Raw_Msg; @@ -88,6 +90,7 @@ public: std::fstream navFile ; // & observables); + /*! + * \brief Writes GLONASS GNAV observables into the RINEX file. Example: glonass_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". + */ + void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & eph, double obs_time, const std::map & observables, const std::string glonass_bands = "1C"); + + /*! + * \brief Writes Mixed GLONASS / GPS L1 C/A observables into the RINEX file + */ + void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Gps_Ephemeris & gps_eph, const double gps_obs_time, const std::map & observables); + + /*! + * \brief Writes Mixed GLONASS / GPS L2C observables into the RINEX file + */ + void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Gps_CNAV_Ephemeris & gps_CNAV_eph, const double gps_obs_time, const std::map & observables); + + /*! + * \brief Writes Mixed GLONASS / Galileo observables into the RINEX file + */ + void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Galileo_Ephemeris & galileo_eph, const double gps_obs_time, const std::map & observables); + /*! * \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not. */ @@ -223,12 +291,22 @@ public: void update_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & utc_model, const Galileo_Almanac & galileo_almanac); + void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); + + void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono); + + void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Gps_CNAV_Utc_Model & gps_CNAV_utc, const Gps_CNAV_Iono & gps_CNAV_iono); + + void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac& galileo_almanac); + void update_obs_header(std::fstream & out, const Gps_Utc_Model & utc_model); void update_obs_header(std::fstream & out, const Gps_CNAV_Utc_Model & utc_model); void update_obs_header(std::fstream & out, const Galileo_Utc_Model & galileo_utc_model); + void update_obs_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); + std::map satelliteSystem; // observationType; // observationCode; // Date: Wed, 19 Jul 2017 07:45:54 -0700 Subject: [PATCH 27/48] Adding RINEX support for GLONASS GNAV processing Adds RINEX v2 and v3 support for GLONASS satellites processing including observation and navigation data records. Also adds conversion of ephemeris records in GNSS-SDR format to RTKLib format --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 141 +- src/algorithms/PVT/libs/rinex_printer.cc | 1445 ++++++++++++++++- src/algorithms/PVT/libs/rinex_printer.h | 39 +- .../libs/rtklib/rtklib_conversions.cc | 59 + .../libs/rtklib/rtklib_conversions.h | 7 + .../glonass_gnav_ephemeris.h | 2 +- 6 files changed, 1551 insertions(+), 142 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index d3fa0af34..1164380c8 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -365,7 +365,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() msgctl(sysv_msqid, IPC_RMID, NULL); //save GPS L2CM ephemeris to XML file - std::string file_name = "eph_GPS_L2CM.xml"; + std::string file_name="eph_GPS_L2CM.xml"; if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) { @@ -377,7 +377,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() ofs.close(); LOG(INFO) << "Saved GPS L2CM Ephemeris map data"; } - catch (const std::exception & e) + catch (std::exception& e) { LOG(WARNING) << e.what(); } @@ -520,7 +520,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite std::map::const_iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); - std::map::const_iterator tmp_eph_iter_glo_gnav = d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); + std::map::const_iterator tmp_eph_iter_glo_gnav = d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); if(((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) @@ -680,12 +680,8 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite * 23 | GLONASS L1 C/A * 24 | GLONASS L2 C/A * 25 | GLONASS L1 C/A + GLONASS L2 C/A - * 26 | GLONASS L1 C/A + GPS L1 C/A - * 27 | GLONASS L1 C/A + GPS L2C - * 28 | GLONASS L1 C/A + GPS L5 - * 29 | GLONASS L1 C/A + Galileo E1B - * 30 | GLONASS L1 C/A + Galileo E5a - * 31 | GLONASS L1 C/A + Galileo E5b + * 26 | GPS L1 C/A + GLONASS L1 C/A + * 27 | Galileo E1B + GLONASS L1 C/A */ // ####################### RINEX FILES ################# @@ -842,53 +838,23 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } } - if(type_of_rx == 26) // GLONASS L1 C/A + GPS L1 C/A - { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) ) - { - std::string gps_signal("1C 1C"); - rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 27) // GLONASS L1 C/A + GPS L2C + if(type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) ) { - std::string gps_signal("1C 1C"); - rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + std::string glo_signal("1C"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); b_rinex_header_written = true; // do not write header anymore } } - if(type_of_rx == 29) // GLONASS L1 C/A + Galileo E1B + if(type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) { - std::string gal_signal("1C 1B"); - rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 30) // GLONASS L1 C/A + Galileo E5a - { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) - { - std::string gal_signal("1C 5X"); - rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 31) // GLONASS L1 C/A + Galileo E5b - { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) - { - std::string gal_signal("1C 7X"); - rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + std::string glo_signal("1C"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); b_rinex_header_written = true; // do not write header anymore } } @@ -925,17 +891,13 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { rp->log_rinex_nav(rp->navGloFile, d_ls_pvt->glonass_gnav_ephemeris_map); } - if((type_of_rx == 26)) // GLONASS L1 C/A + GPS L1 C/A + if((type_of_rx == 26)) // GPS L1 C/A + GLONASS L1 C/A { - rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->gps_ephemeris_map); + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->glonass_gnav_ephemeris_map); } - if((type_of_rx == 27)) // GLONASS L1 C/A + GPS L2C + if((type_of_rx == 27)) // Galileo E1B + GLONASS L1 C/A { - rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->gps_cnav_ephemeris_map); - } - if((type_of_rx == 29) || (type_of_rx == 30) || (type_of_rx == 31)) // GLONASS L1 C/A + Galileo - { - rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->galileo_ephemeris_map, d_ls_pvt->glonass_gnav_ephemeris_map); } } galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); @@ -1102,78 +1064,33 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_updated = true; } } - if(type_of_rx == 26) // GLONASS L1 C/A + GPS L1 C/A + if(type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) { - std::string gps_signal("1C 1C"); - rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + std::string glo_signal("1C"); + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) { - rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); b_rinex_header_updated = true; // do not write header anymore } } - if(type_of_rx == 27) // GLONASS L1 C/A + GPS L1 C/A - { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) - { - std::string gps_signal("1C 2S"); - rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gps_signal); - - } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) - { - rp->update_obs_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); - b_rinex_header_updated = true; // do not write header anymore - } - } - if(type_of_rx == 29) // GLONASS L1 C/A + Galileo E1B + if(type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) { - std::string gal_signal("1C 1B"); - rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + std::string glo_signal("1C"); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { - rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_updated = true; // do not write header anymore - } - } - if(type_of_rx == 30) // GLONASS L1 C/A + Galileo E5a - { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) - { - std::string gal_signal("1C 5X"); - rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - - } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_updated = true; // do not write header anymore - } - } - if(type_of_rx == 31) // GLONASS L1 C/A + Galileo E5b - { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) - { - std::string gal_signal("1C 7X"); - rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - - } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); b_rinex_header_updated = true; // do not write header anymore } } diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 4631270d2..4dfcbe4cc 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -58,12 +58,14 @@ Rinex_Printer::Rinex_Printer(int conf_version) sbsfilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_SBAS"); navGalfilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_GAL_NAV"); navMixfilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_MIXED_NAV"); + navGlofilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_GLO_NAV"); Rinex_Printer::navFile.open(navfilename, std::ios::out | std::ios::in | std::ios::app); Rinex_Printer::obsFile.open(obsfilename, std::ios::out | std::ios::in | std::ios::app); Rinex_Printer::sbsFile.open(sbsfilename, std::ios::out | std::ios::app); Rinex_Printer::navGalFile.open(navGalfilename, std::ios::out | std::ios::in | std::ios::app); Rinex_Printer::navMixFile.open(navMixfilename, std::ios::out | std::ios::in | std::ios::app); + Rinex_Printer::navGloFile.open(navGlofilename, std::ios::out | std::ios::in | std::ios::app); // RINEX v3.02 codes satelliteSystem["GPS"] = "G"; @@ -200,10 +202,13 @@ Rinex_Printer::~Rinex_Printer() poss = sbsFile.tellp(); posng = navGalFile.tellp(); posmn = navMixFile.tellp(); + posnr = navGalFile.tellp(); + Rinex_Printer::navFile.close(); Rinex_Printer::obsFile.close(); Rinex_Printer::sbsFile.close(); Rinex_Printer::navGalFile.close(); + Rinex_Printer::navGloFile.close(); // If nothing written, erase the files. if (posn == 0) { @@ -225,6 +230,10 @@ Rinex_Printer::~Rinex_Printer() { if(remove(navMixfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; } + if (posnr == 0) + { + if(remove(navGlofilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; + } } @@ -399,6 +408,226 @@ std::string Rinex_Printer::getLocalTime() } +void Rinex_Printer::rinex_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) +{ + std::string line; + stringVersion = "3.02"; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += stringVersion; + line += std::string(11, ' '); + line += std::string("N: GNSS NAV DATA"); + line += std::string(4, ' '); + line += std::string("R: GLONASS"); + line += std::string(10, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("GLONASS NAVIGATION MESSAGE FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction + line.clear(); + line += std::string("GLUT"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_c, 16, 2), 17); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction 2 + line.clear(); + line += std::string("GLGP"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_gps, 16, 2), 17); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + // -------- Line 6 leap seconds + // For leap second information, see http://www.endruntechnologies.com/leap.htm + line.clear(); + // TODO hOW TO MAKE THIS IN GLONASS, the leap second is internally given + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 6); + line += std::string(36, ' '); + line += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- End of Header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + +void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) +{ + std::string line; + stringVersion = "3.02"; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += stringVersion; + line += std::string(11, ' '); + line += std::string("N: GNSS NAV DATA"); + line += std::string(4, ' '); + line += std::string("M: MIXED"); + line += std::string(12, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("GNSS NAVIGATION MESSAGE FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line ionospheric info 1 + line.clear(); + line += std::string("GPSA"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha0, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha1, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha2, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha3, 10, 2), 12); + line += std::string(7, ' '); + line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction 1 + line.clear(); + line += std::string("GLUT"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_c, 16, 2), 17); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction 2 + line.clear(); + line += std::string("GLGP"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_gps, 16, 2), 17); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction 3 + line.clear(); + line += std::string("GPUT"); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_utc_model.d_A0, 16, 2), 18); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_utc_model.d_A1, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.d_t_OT), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.i_WN_T + 1024), 5); // valid until 2019 + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + // -------- Line 6 leap seconds + // For leap second information, see http://www.endruntechnologies.com/leap.htm + line.clear(); + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.d_DeltaT_LS), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.d_DeltaT_LSF), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.i_WN_LSF), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.i_DN), 6); + line += std::string(36, ' '); + line += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- End of Header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac) { std::string line; @@ -1048,6 +1277,89 @@ void Rinex_Printer::rinex_sbs_header(std::fstream& out) } +void Rinex_Printer::update_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) +{ + std::vector data; + std::string line_aux; + + long pos = out.tellp(); + out.seekp(0); + data.clear(); + + bool no_more_finds = false; + std::string line_str; + + while(!out.eof()) + { + std::getline(out, line_str); + + if(!no_more_finds) + { + line_aux.clear(); + + if ((line_str.find("GLUT", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GLUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_c, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GLGP", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GLGP"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_gps, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if (line_str.find("LEAP SECONDS", 59) != std::string::npos) + { + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.Delta_tLS_6), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.Delta_tLSF_6), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.WN_LSF_6), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.DN_6), 6); + line_aux += std::string(36, ' '); + line_aux += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + data.push_back(line_aux); + } + else if (line_str.find("END OF HEADER", 59) != std::string::npos) + { + data.push_back(line_str); + no_more_finds = true; + } + else + { + data.push_back(line_str); + } + + } + else + { + data.push_back(line_str); + } + } + + out.close(); + out.open(navGlofilename, std::ios::out | std::ios::trunc); + out.seekp(0); + for (int i = 0; i < static_cast(data.size()) - 1; i++) + { + out << data[i] << std::endl; + } + out.close(); + out.open(navGlofilename, std::ios::out | std::ios::app); + out.seekp(pos); + std::cout << "The RINEX Navigation file header has been updated with UTC info." << std::endl; +} + + void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac) { std::vector data; @@ -1133,7 +1445,7 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& gal out.close(); out.open(navGalfilename, std::ios::out | std::ios::trunc); out.seekp(0); - for (int i = 0; i < static_cast(data.size()) - 1; i++) + for (int i = 0; i < (int) data.size() - 1; i++) { out << data[i] << std::endl; } @@ -1521,6 +1833,112 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion } +void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) +{ + std::vector data; + std::string line_aux; + + long pos = out.tellp(); + out.seekp(0); + data.clear(); + + bool no_more_finds = false; + std::string line_str; + + while(!out.eof()) + { + std::getline(out, line_str); + + if(!no_more_finds) + { + line_aux.clear(); + + if (line_str.find("GPSA", 0) != std::string::npos) + { + line_aux += std::string("GPSA"); + line_aux += std::string(1, ' '); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha0, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha1, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha2, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_iono.d_alpha3, 10, 2), 12); + line_aux += std::string(7, ' '); + line_aux += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GPUT", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GPUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_utc_model.d_A0, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(gps_utc_model.d_A1, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.d_t_OT), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.i_WN_T + 1024), 5); // valid until 2019 + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GLUT", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GLUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_c, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GLGP", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GLGP"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_gps, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if (line_str.find("LEAP SECONDS", 59) != std::string::npos) + { + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.d_DeltaT_LS), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.d_DeltaT_LSF), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.i_WN_LSF), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(gps_utc_model.i_DN), 6); + line_aux += std::string(36, ' '); + line_aux += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + data.push_back(line_aux); + } + else if (line_str.find("END OF HEADER", 59) != std::string::npos) + { + data.push_back(line_str); + no_more_finds = true; + } + else + { + data.push_back(line_str); + } + + } + else + { + data.push_back(line_str); + } + } + + out.close(); + out.open(navMixfilename, std::ios::out | std::ios::trunc); + out.seekp(0); + for (int i = 0; i < (int) data.size() - 1; i++) + { + out << data[i] << std::endl; + } + out.close(); + out.open(navMixfilename, std::ios::out | std::ios::app); + out.seekp(pos); + std::cout << "The RINEX Navigation file header has been updated with UTC and IONO info." << std::endl; +} + + void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& eph_map) { std::string line; @@ -2194,6 +2612,200 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& eph_map) +{ + std::string line; + std::map::const_iterator glonass_gnav_ephemeris_iter; + + for(glonass_gnav_ephemeris_iter = eph_map.begin(); + glonass_gnav_ephemeris_iter != eph_map.end(); + glonass_gnav_ephemeris_iter++) + { + // -------- SV / EPOCH / SV CLK + boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_GLONASS_time(glonass_gnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second.d_Toc); + std::string timestring = boost::posix_time::to_iso_string(p_utc_time); + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + std::string seconds (timestring, 13, 2); + if (version == 2) + { + line += Rinex_Printer::rightJustify(boost::lexical_cast(gps_ephemeris_iter->second.i_satellite_PRN), 2); + line += std::string(1, ' '); + std::string year (timestring, 2, 2); + line += year; + line += std::string(1, ' '); + if(boost::lexical_cast(month) < 10) + { + line += std::string(1, ' '); + line += std::string(month, 1, 1); + } + else + { + line += month; + } + line += std::string(1, ' '); + if(boost::lexical_cast(day) < 10) + { + line += std::string(1, ' '); + line += std::string(day, 1, 1); + } + else + { + line += day; + } + line += std::string(1, ' '); + if(boost::lexical_cast(hour) < 10) + { + line += std::string(1, ' '); + line += std::string(hour, 1, 1); + } + else + { + line += hour; + } + line += std::string(1, ' '); + if(boost::lexical_cast(minutes) < 10) + { + line += std::string(1, ' '); + line += std::string(minutes, 1, 1); + } + else + { + line += minutes; + } + line += std::string(1, ' '); + if(boost::lexical_cast(seconds) < 10) + { + line += std::string(1, ' '); + line += std::string(seconds, 1, 1); + } + else + { + line += seconds; + } + + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(-glonass_gnav_ephemeris_iter->second.d_tau_c, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_gamma_n, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_t_k, 18, 2); + line += std::string(1, ' '); + } + if (version == 3) + { + line += satelliteSystem["GLONASS"]; + if (glonass_gnav_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0"); + line += boost::lexical_cast(glonass_gnav_ephemeris_iter->second.i_satellite_PRN); + std::string year (timestring, 0, 4); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + line += std::string(1, ' '); + line += seconds; + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(-glonass_gnav_ephemeris_iter->second.d_tau_n, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(+glonass_gnav_ephemeris_iter->second.d_gamma_n, 18, 2); + line += std::string(1, ' '); + //TODO need to define this here. what is nd + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_t_k + nd*86400, 18, 2); + } + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + // -------- BROADCAST ORBIT - 1 + line.clear(); + // TODO Why is this happening here?. The extra space maybe is intended to help with readability + if (version == 2) + { + line += std::string(3, ' '); + } + if (version == 3) + { + line += std::string(4, ' '); + } + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_Xn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_VXn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_AXn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_B_n, 18, 2); + if (version == 2) + { + line += std::string(1, ' '); + } + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + // -------- BROADCAST ORBIT - 2 + line.clear(); + if (version == 2) + { + line += std::string(3, ' '); + } + if (version == 3) + { + line += std::string(4, ' '); + } + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_Yn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_VYn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_ZYn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.i_satellite_freq_channel, 18, 2); + if (version == 2) + { + line += std::string(1, ' '); + } + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + + // -------- BROADCAST ORBIT - 3 + line.clear(); + if (version == 2) + { + line += std::string(3, ' '); + } + if (version == 3) + { + line += std::string(4, ' '); + } + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_Zn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_VZn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_AZn, 18, 2); + line += std::string(1, ' '); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_E_n, 18, 2); + if (version == 2) + { + line += std::string(1, ' '); + } + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + } +} + + void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& gps_eph_map, const std::map& galileo_eph_map) { version = 3; @@ -2203,6 +2815,584 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& gps_eph_map, const std::map& glonass_gnav_eph_map) +{ + version = 3; + stringVersion = "3.02"; + Rinex_Printer::log_rinex_nav(out, gps_eph_map); + Rinex_Printer::log_rinex_nav(out, glonass_gnav_eph_map); +} + + +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, const double d_TOW_first_observation) +{ + std::string line; + std::map::const_iterator glonass_gnav_ephemeris_iter; + + // -------- Line 1 + line = std::string(5, ' '); + line += stringVersion; + line += std::string(11, ' '); + line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); + line += satelliteSystem["GLONASS"]; + line += std::string(19, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + if (version == 2) + { + line += Rinex_Printer::leftJustify("BLANK OR G = GPS, R = GLONASS, E = GALILEO, M = MIXED", 60); + } + if (version == 3) + { + line += Rinex_Printer::leftJustify("G = GPS R = GLONASS E = GALILEO S = GEO M = MIXED", 60); + } + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 3 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("GLONASS OBSERVATION DATA FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER NAME + line.clear(); + line += Rinex_Printer::leftJustify("DEFAULT MARKER NAME", 60); // put a flag or a property, + line += Rinex_Printer::leftJustify("MARKER NAME", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER TYPE + line.clear(); + line += Rinex_Printer::leftJustify("GROUND_CRAFT", 20); // put a flag or a property + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("MARKER TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line OBSERVER / AGENCY + line.clear(); + std::string username; + char c_username[20] = {0}; + int nGet = getlogin_r(c_username, sizeof(c_username) - 1); + if (nGet == 0) + { + username = c_username; + } + else + { + username = "UNKNOWN USER"; + } + line += leftJustify(username, 20); + line += Rinex_Printer::leftJustify("CTTC", 40); // add flag and property + line += Rinex_Printer::leftJustify("OBSERVER / AGENCY", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line REC / TYPE VERS + line.clear(); + line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property + line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property + //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property + if(gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); + line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); + line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); + lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA TYPE + line.clear(); + line += Rinex_Printer::leftJustify("Antenna number", 20); // add flag and property + line += Rinex_Printer::leftJustify("Antenna type", 20); // add flag and property + line += std::string(20, ' '); + line += Rinex_Printer::leftJustify("ANT # / TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- APPROX POSITION (optional for moving platforms) + // put here real data! + double antena_x = 0.0; + double antena_y = 0.0; + double antena_z = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_x, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_y, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_z, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("APPROX POSITION XYZ", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h = 0.0; + double antena_e = 0.0; + double antena_n = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_h, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_e, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("ANTENNA: DELTA H/E/N", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + if (version == 2) + { + // --------- WAVELENGHT FACTOR + // put here real data! + line.clear(); + line +=Rinex_Printer::rightJustify("1",6); + line +=Rinex_Printer::rightJustify("1",6); + line += std::string(48, ' '); + line += Rinex_Printer::leftJustify("WAVELENGTH FACT L1/2", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + } + + if (version == 3) + { + // -------- SYS / OBS TYPES + // one line per available system + line.clear(); + line += satelliteSystem["GLONASS"]; + line += std::string(2, ' '); + std::stringstream strm; + numberTypesObservations = 4; + strm << numberTypesObservations; + line += Rinex_Printer::rightJustify(strm.str(), 3); + // per type of observation + // GLONASS L1 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G1_CA"]; + // GLONASS L1 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GLONASS_G1_CA"]; + // GLONASS DOPPLER L1 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GLONASS_G1_CA"]; + // GLONASS L1 CA SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GLONASS_G1_CA"]; + + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + } + + if (version == 2) + { + // -------- SYS / OBS TYPES + line.clear(); + std::stringstream strm; + strm << numberTypesObservations; + line += Rinex_Printer::rightJustify(strm.str(), 6); + // per type of observation + // GLONASS L1 C/A PSEUDORANGE + line += Rinex_Printer::rightJustify(observationType["PSEUDORANGE_CA_v2"], 5); + line += observationCode["GLONASS_G1_CA_v2"]; + // GLONASS L1 PHASE + line += Rinex_Printer::rightJustify(observationType["CARRIER_PHASE_CA_v2"], 5); + line += observationCode["GLONASS_G1_CA_v2"]; + // GLONASS DOPPLER L1 + line += Rinex_Printer::rightJustify(observationType["DOPPLER_v2"], 5); + line += observationCode["GLONASS_G1_CA_v2"]; + // GLONASS L1 SIGNAL STRENGTH + line += Rinex_Printer::rightJustify(observationType["SIGNAL_STRENGTH_v2"], 5); + line += observationCode["GLONASS_G1_CA_v2"]; + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("# / TYPES OF OBSERV", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + } + + if (version == 3) + { + // -------- Signal Strength units + line.clear(); + line += Rinex_Printer::leftJustify("DBHZ", 20); + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("SIGNAL STRENGTH UNIT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + } + + // -------- TIME OF FIRST OBS + line.clear(); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph,d_TOW_first_observation); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + std::string year (timestring, 0, 4); + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + double gps_t = d_TOW_first_observation; + double seconds = fmod(gps_t, 60); + line += Rinex_Printer::rightJustify(year, 6); + line += Rinex_Printer::rightJustify(month, 6); + line += Rinex_Printer::rightJustify(day, 6); + line += Rinex_Printer::rightJustify(hour, 6); + line += Rinex_Printer::rightJustify(minutes, 6); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); + line += Rinex_Printer::rightJustify(std::string("GLO"), 8); + line += std::string(9, ' '); + line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- SYS /PHASE SHIFTS + // -------- GLONASS SLOT / FRQ # + line.clear(); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 3); + line += std::string(1, ' '); + + // TODO Add this here, we need to know all satellites in system to get this done + line += satelliteSystem["GLONASS"]; + line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 2); // Slot Number + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 2); // Frequency Number + line += std::string(1, ' '); + + + line += Rinex_Printer::leftJustify("GLONASS SLOT / FRQ #", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- GLONASS CODE/PHS/BIS + // TODO This needs more study, not really suer on what those values are. Setting it to zero to advance in code. + line.clear(); + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G1_CA"]; + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(asString(0.0, 3), 8); + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G1_P"]; + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(asString(0.0, 3), 8); + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G2_CA"]; + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(asString(0.0, 3), 8); + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G2_P"]; + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(asString(0.0, 3), 8); + line += Rinex_Printer::leftJustify("GLONASS SLOT / FRQ #", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- END OF HEADER + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double d_TOW_first_observation, const std::string glonass_bands) +{ + std::string line; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += "3.02"; + line += std::string(11, ' '); + line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); + line += satelliteSystem["Mixed"]; + line += std::string(19, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::leftJustify("G = GPS R = GLONASS E = GALILEO S = GEO M = MIXED", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 3 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("MIXED (GPS/GLONASS) OBSERVATION DATA FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER NAME + line.clear(); + line += Rinex_Printer::leftJustify("DEFAULT MARKER NAME", 60); // put a flag or a property, + line += Rinex_Printer::leftJustify("MARKER NAME", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER TYPE + line.clear(); + line += Rinex_Printer::leftJustify("NON_GEODETIC", 20); // put a flag or a property + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("MARKER TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line OBSERVER / AGENCY + line.clear(); + std::string username; + char c_username[20] = {0}; + int nGet = getlogin_r(c_username, sizeof(c_username) - 1); + if (nGet == 0) + { + username = c_username; + } + else + { + username = "UNKNOWN USER"; + } + line += leftJustify(username, 20); + line += Rinex_Printer::leftJustify("CTTC", 40); // add flag and property + line += Rinex_Printer::leftJustify("OBSERVER / AGENCY", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line REC / TYPE VERS + line.clear(); + line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property + line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property + //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property + if(gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); + line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); + line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); + lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA TYPE + line.clear(); + line += Rinex_Printer::leftJustify("Antenna number", 20); // add flag and property + line += Rinex_Printer::leftJustify("Antenna type", 20); // add flag and property + line += std::string(20, ' '); + line += Rinex_Printer::leftJustify("ANT # / TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- APPROX POSITION (optional for moving platforms) + // put here real data! + double antena_x = 0.0; + double antena_y = 0.0; + double antena_z = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_x, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_y, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_z, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("APPROX POSITION XYZ", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h = 0.0; + double antena_e = 0.0; + double antena_n = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_h, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_e, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("ANTENNA: DELTA H/E/N", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- SYS / OBS TYPES + // one line per available system + line.clear(); + line += satelliteSystem["GPS"]; + line += std::string(2, ' '); + std::stringstream strm; + numberTypesObservations = 4; + strm << numberTypesObservations; + line += Rinex_Printer::rightJustify(strm.str(), 3); + // per type of observation + // GPS L1 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L1_CA"]; + // GPS DOPPLER L1 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 CA SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L1_CA"]; + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // Find GLONASS Signal in Mixed file + unsigned int number_of_observations_glo = 0; + std::string signal_("1C"); + std::size_t found_1C = glonass_bands.find(signal_); + if(found_1C != std::string::npos) + { + number_of_observations_glo = number_of_observations_glo + 4; + } + signal_ = "2C"; + std::size_t found_2C = galileo_bands.find(signal_); + if(found_2C != std::string::npos) + { + number_of_observations_glo = number_of_observations_glo + 4; + } + + line.clear(); + line += satelliteSystem["GLONASS"]; + line += std::string(2, ' '); + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_glO), 3); + + if(found_1C != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G1_CA"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GLONASS_G1_CA"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GLONASS_G1_CA"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GLONASS_G1_CA"]; + } + + if(found_2C != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_G2_CA"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GLONASS_G2_CA"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GLONASS_G2_CA"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GLONASS_G2_CA"]; + } + + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Signal Strength units + line.clear(); + line += Rinex_Printer::leftJustify("DBHZ", 20); + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("SIGNAL STRENGTH UNIT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- TIME OF FIRST OBS + line.clear(); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(gps_eph, d_TOW_first_observation); + std::string timestring=boost::posix_time::to_iso_string(p_gps_time); + std::string year (timestring, 0, 4); + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + double gps_t = d_TOW_first_observation; + double seconds = fmod(gps_t, 60); + line += Rinex_Printer::rightJustify(year, 6); + line += Rinex_Printer::rightJustify(month, 6); + line += Rinex_Printer::rightJustify(day, 6); + line += Rinex_Printer::rightJustify(hour, 6); + line += Rinex_Printer::rightJustify(minutes, 6); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); + line += Rinex_Printer::rightJustify(std::string("GPS"), 8); + line += std::string(9, ' '); + line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- end of header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const double d_TOW_first_observation) { std::string line; @@ -3651,6 +4841,249 @@ void Rinex_Printer::update_obs_header(std::fstream& out, const Galileo_Utc_Model } +void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonassps_Gnav_Ephemeris& eph, const double obs_time, const std::map& observables) +{ + // RINEX observations timestamps are GPS timestamps. + std::string line; + + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph, obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); + //double gps_t = eph.sv_clock_correction(obs_time); + double gps_t = obs_time; + + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + + if (version == 2) + { + line.clear(); + std::string year (timestring, 2, 2); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + if (month.compare(0, 1 , "0") == 0) + { + line += std::string(1, ' '); + line += month.substr(1, 1); + } + else + { + line += month; + } + line += std::string(1, ' '); + if (day.compare(0, 1 , "0") == 0) + { + line += std::string(1, ' '); + line += day.substr(1, 1); + } + else + { + line += day; + } + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + line += std::string(1, ' '); + double second_ = fmod(gps_t, 60); + if (second_ < 10) + { + line += std::string(1, ' '); + } + line += Rinex_Printer::asString(second_, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + //Number of satellites observed in current epoch + int numSatellitesObserved = 0; + std::map::const_iterator observables_iter; + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + numSatellitesObserved++; + } + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + line += satelliteSystem["GLONASS"]; + if (static_cast(observables_iter->second.PRN) < 10) line += std::string(1, '0'); + line += boost::lexical_cast(static_cast(observables_iter->second.PRN)); + } + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string lineObs; + lineObs.clear(); + line.clear(); + // GPS L1 PSEUDORANGE + line += std::string(2, ' '); + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(observables_iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + // GPS L1 CA PHASE + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads/GPS_TWO_PI, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + // GPS L1 CA DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + //GPS L1 SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } + } + + if (version == 3) + { + std::string year (timestring, 0, 4); + line += std::string(1, '>'); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + + line += std::string(1, ' '); + double seconds = fmod(gps_t, 60); + // Add extra 0 if seconds are < 10 + if (seconds < 10) + { + line += std::string(1, '0'); + } + line += Rinex_Printer::asString(seconds, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + + //Number of satellites observed in current epoch + int numSatellitesObserved = 0; + std::map::const_iterator observables_iter; + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + numSatellitesObserved++; + } + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + + + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string lineObs; + lineObs.clear(); + lineObs += satelliteSystem["GLONASS"]; + if (static_cast(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(observables_iter->second.PRN)); + //lineObs += std::string(2, ' '); + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(observables_iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS L1 CA PHASE + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads/GLONASS_TWO_PI, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS L1 CA DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + //GLONASS L1 SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } + } +} + + void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, const double obs_time, const std::map& observables) { // RINEX observations timestamps are GPS timestamps. @@ -4909,6 +6342,16 @@ boost::posix_time::ptime Rinex_Printer::compute_Galileo_time(const Galileo_Ephem } +boost::posix_time::ptime Rinex_Printer::compute_GLONASS_time(const Glonass_Gnav_Ephemeris& eph, const double obs_time) +{ + // The RINEX v2.11 v3.00 format uses Galileo time for the observations epoch, not UTC time, thus, no leap seconds needed here. + // (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf) + // --??? No time correction here, since it will be done in the RINEX processor + double glonass_t = obs_time; + boost::posix_time::time_duration t = boost::posix_time::millisec((galileo_t + 604800 * static_cast(eph.WN_5)) * 1000); // + boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); + return p_time; +} /* enum RINEX_enumMarkerType { diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index c4029f868..7264a32d2 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -119,19 +119,14 @@ public: void rinex_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); /*! - * \brief Generates the Mixed (GLONASS/Galileo) Navigation Data header + * \brief Generates the Mixed (Galileo/GLONASS) Navigation Data header */ - void rinex_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac & galileo_almanac); + void rinex_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac & galileo_almanac, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); /*! * \brief Generates the Mixed (GLONASS L1, L2/GPS L1 C/A) Navigation Data header */ - void rinex_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc_model); - - /*! - * \brief Generates the Mixed (GLONASS L1, L2/GPS L2C ) Navigation Data header - */ - void rinex_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Gps_CNAV_Iono & gps_CNAV_iono, const Gps_CNAV_Utc_Model & gps_CNAV_utc_model); + void rinex_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc_model, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); /*! * \brief Generates the GPS Observation data header @@ -166,17 +161,12 @@ public: /*! * \brief Generates the Mixed (GPS L1 C/A /GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". */ - void rinex_obs_header(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Gps_Ephemeris & gps_eph, const double d_TOW_first_observation, const std::string gps_bands = "1C"); - - /*! - * \brief Generates the Mixed (GPS L2C/GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". - */ - void rinex_obs_header(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Gps_Ephemeris & gps_CNAV_eph, const double d_TOW_first_observation, const std::string gps_bands = "1C"); + void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & gps_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1C"); /*! * \brief Generates the Mixed (Galileo/GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". */ - void rinex_obs_header(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Galileo_Ephemeris & galileo_eph, const double d_TOW_first_observation, const std::string gps_bands = "1B"); + void rinex_obs_header(std::fstream & out, const Galileo_Ephemeris & galileo_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1C"); /*! * \brief Generates the SBAS raw data header @@ -259,19 +249,14 @@ public: void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & eph, double obs_time, const std::map & observables, const std::string glonass_bands = "1C"); /*! - * \brief Writes Mixed GLONASS / GPS L1 C/A observables into the RINEX file + * \brief Writes Mixed GPS L1 C/A - GLONASS observables into the RINEX file */ - void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Gps_Ephemeris & gps_eph, const double gps_obs_time, const std::map & observables); + void log_rinex_obs(std::fstream & out, const Gps_Ephemeris & gps_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double gps_obs_time, const std::map & observables); /*! - * \brief Writes Mixed GLONASS / GPS L2C observables into the RINEX file + * \brief Writes Mixed Galileo/GLONASS observables into the RINEX file */ - void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Gps_CNAV_Ephemeris & gps_CNAV_eph, const double gps_obs_time, const std::map & observables); - - /*! - * \brief Writes Mixed GLONASS / Galileo observables into the RINEX file - */ - void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Galileo_Ephemeris & galileo_eph, const double gps_obs_time, const std::map & observables); + void log_rinex_obs(std::fstream & out, const Galileo_Ephemeris & galileo_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double gps_obs_time, const std::map & observables); /*! * \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not. @@ -293,11 +278,9 @@ public: void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); - void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono); + void update_nav_header(std::fstream & out, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); - void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Gps_CNAV_Utc_Model & gps_CNAV_utc, const Gps_CNAV_Iono & gps_CNAV_iono); - - void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac& galileo_almanac); + void update_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void update_obs_header(std::fstream & out, const Gps_Utc_Model & utc_model); diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index c5e673b9c..a2374a67d 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -51,6 +51,10 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch case 'E': rtklib_obs.sat = gnss_synchro.PRN+NSATGPS+NSATGLO; break; + case 'R': + rtklib_obs.sat = gnss_synchro.PRN; + break; + default: rtklib_obs.sat = gnss_synchro.PRN; } @@ -60,6 +64,61 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch return rtklib_obs; } +geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + geph_t rtklib_sat = {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, 0.0}, 0.0, 0.0, 0.0}; + + int iode; /* IODE (0-6 bit of tb field) */ + int frq; /* satellite frequency number */ + int svh,sva,age; /* satellite health, accuracy, age of operation */ + gtime_t toe; /* epoch of epherides (gpst) */ + gtime_t tof; /* message frame time (gpst) */ + double pos[3]; /* satellite position (ecef) (m) */ + double vel[3]; /* satellite velocity (ecef) (m/s) */ + double acc[3]; /* satellite acceleration (ecef) (m/s^2) */ + double taun,gamn; /* SV clock bias (s)/relative freq bias */ + double dtaun; + + rtklib_sat.sat = glonass_gnav_eph.i_satellite_PRN; /* satellite number */ + rtklib_sat.iode = glonass_gnav_eph.d_sqrt_A; + rtklib_sat.frq = glonass_gnav_eph.; + rtklib_sat.svh = glonass_gnav_eph.; + rtklib_sat.sva = glonass_gnav_eph. + rtklib_sat.age = glonass_gnav_eph. + + rtklib_sat.toe = gpst2time(rtklib_sat.week, gps_eph.d_Toc); + rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_eph.d_TOW); + + rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn; + rtklib_sat.pos[1] = glonass_gnav_eph.d_Yn; + rtklib_sat.pos[2] = glonass_gnav_eph.d_Zn; + + rtklib_sat.vel[0] = glonass_gnav_eph.d_VXn; + rtklib_sat.vel[1] = glonass_gnav_eph.d_VYn; + rtklib_sat.vel[2] = glonass_gnav_eph.d_VZn; + + rtklib_sat.acc[0] = glonass_gnav_eph.d_AXn; + rtklib_sat.acc[1] = glonass_gnav_eph.d_AYn; + rtklib_sat.acc[2] = glonass_gnav_eph.d_AZn; + + rtklib_sat.taun = glonass_gnav_eph.d_tau_n; /* SV clock bias (s) */ + rtklib_sat.gann = glonass_gnav_eph.d_gamma_n; /* SV relative freq bias */ + rtklib_sat.gann = glonass_gnav_eph.d_gamma_n; + + /* adjustment for week handover */ + double tow, toc; + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + + return rtklib_sat; +} + eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 18336754f..e620adaaa 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -36,10 +36,17 @@ #include "galileo_ephemeris.h" #include "gps_ephemeris.h" #include "gps_cnav_ephemeris.h" +#include "glonass_gnav_ephemeris.h" eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph); eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph); eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph); +/*! + * \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart + * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure + * \return Ephemeris structure for RTKLIB parsing + */ +geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synchro, int week, int band); diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index d0edcc69b..6348270a8 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -68,7 +68,7 @@ private: double d_Jz_sun; //!< Sun gravitational perturbation public: - unsigned int i_satellite_freq_channel; // SV Frequency Channel Number + int i_satellite_freq_channel; // SV Frequency Channel Number double d_m; //!< String number within frame [dimensionless] double d_t_k; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] double d_t_b; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] From d34ec96ad2927b0629fbb4c2958d0856e76d8a52 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 23 Jul 2017 22:57:54 -0700 Subject: [PATCH 28/48] bugfix: Fixing bugs with RINEX support Fixes compilation errors. Modifies GLONASS GNAV Ephemeris object to contain fields relative to GPS time using the boost posix time object. Adds unit tests to check existent and recently added code --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 21 +- src/algorithms/PVT/libs/rinex_printer.cc | 1055 ++++++++++++++++- src/algorithms/PVT/libs/rinex_printer.h | 21 +- src/algorithms/PVT/libs/rtklib_solver.cc | 5 +- .../libs/rtklib/rtklib_conversions.cc | 82 +- src/core/system_parameters/GLONASS_L1_CA.h | 8 +- .../system_parameters/glonass_gnav_almanac.cc | 1 + .../system_parameters/glonass_gnav_almanac.h | 4 +- .../glonass_gnav_ephemeris.cc | 12 + .../glonass_gnav_ephemeris.h | 112 +- .../glonass_gnav_navigation_message.cc | 226 ++-- .../glonass_gnav_navigation_message.h | 11 +- .../glonass_gnav_utc_model.h | 2 +- src/core/system_parameters/gps_ephemeris.h | 1 + 14 files changed, 1320 insertions(+), 241 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 1164380c8..e28e35b01 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -188,9 +188,9 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) glonass_gnav_eph = boost::any_cast>(pmt::any_ref(msg)); // TODO Add GLONASS with gps week number and tow, // insert new ephemeris record - DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->TOW_5 - << ", GLONASS GNAV Week Number =" << glonass_gnav_eph->WN_5 - << " and Ephemeris IOD = " << glonass_gnav_eph->IOD_ephemeris; + DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW + << ", GLONASS GNAV Week Number =" << glonass_gnav_eph->d_WN + << " and Ephemeris IOD = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b); // update/insert new ephemeris record to the global ephemeris map d_ls_pvt->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; } @@ -200,7 +200,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) std::shared_ptr glonass_gnav_utc_model; glonass_gnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); d_ls_pvt->glonass_gnav_utc_model = *glonass_gnav_utc_model; - DLOG(INFO) << "New UTC record has arrived "; + DLOG(INFO) << "New GLONASS GNAV UTC record has arrived "; } else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) { @@ -557,7 +557,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { if(tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) { - d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + //d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } } @@ -853,7 +853,8 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) { std::string glo_signal("1C"); - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); + std::string gal_signal("1B"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -1068,9 +1069,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) { - std::string glo_signal("1C"); - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); - + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) { @@ -1081,10 +1080,10 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } if(type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) { std::string glo_signal("1C"); - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 4dfcbe4cc..7c51b8c95 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -196,13 +196,13 @@ Rinex_Printer::Rinex_Printer(int conf_version) Rinex_Printer::~Rinex_Printer() { // close RINEX files - long posn, poso, poss, posng, posmn; + long posn, poso, poss, posng, posmn, posnr; posn = navFile.tellp(); poso = obsFile.tellp(); poss = sbsFile.tellp(); posng = navGalFile.tellp(); posmn = navMixFile.tellp(); - posnr = navGalFile.tellp(); + posnr = navGloFile.tellp(); Rinex_Printer::navFile.close(); Rinex_Printer::obsFile.close(); @@ -628,6 +628,116 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono } +void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) +{ + std::string line; + stringVersion = "3.02"; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += stringVersion; + line += std::string(11, ' '); + line += std::string("N: GNSS NAV DATA"); + line += std::string(4, ' '); + line += std::string("M: MIXED"); + line += std::string(12, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("GNSS NAVIGATION MESSAGE FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line ionospheric info 1 + line.clear(); + line += std::string("GAL "); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai0_5, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai1_5, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai2_5, 10, 2), 12); + double zero = 0.0; + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(zero, 10, 2), 12); + line += std::string(7, ' '); + line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + + // -------- Line system time correction + line.clear(); + line += std::string("GAUT"); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A0_6, 16, 2), 18); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A1_6, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.t0t_6), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WNot_6), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction 1 + line.clear(); + line += std::string("GLUT"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_c, 16, 2), 17); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 6 leap seconds + // For leap second information, see http://www.endruntechnologies.com/leap.htm + line.clear(); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.Delta_tLS_6), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.Delta_tLSF_6), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WN_LSF_6), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.DN_6), 6); + line += std::string(36, ' '); + line += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- End of Header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac) { std::string line; @@ -1319,16 +1429,6 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Glonass_Gnav_Utc_ line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); data.push_back(line_aux); } - else if (line_str.find("LEAP SECONDS", 59) != std::string::npos) - { - line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.Delta_tLS_6), 6); - line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.Delta_tLSF_6), 6); - line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.WN_LSF_6), 6); - line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.DN_6), 6); - line_aux += std::string(36, ' '); - line_aux += Rinex_Printer::leftJustify("LEAP SECONDS", 20); - data.push_back(line_aux); - } else if (line_str.find("END OF HEADER", 59) != std::string::npos) { data.push_back(line_str); @@ -1939,6 +2039,102 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion } +void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) +{ + std::vector data; + std::string line_aux; + + long pos = out.tellp(); + out.seekp(0); + data.clear(); + + bool no_more_finds = false; + std::string line_str; + + while(!out.eof()) + { + std::getline(out, line_str); + + if(!no_more_finds) + { + line_aux.clear(); + + if ((line_str.find("GAL", 0) != std::string::npos) && (line_str.find("IONOSPHERIC CORR", 59) != std::string::npos)) + { + line_aux += std::string("GAL "); + line_aux += std::string(1, ' '); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai0_5, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai1_5, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai2_5, 10, 2), 12); + double zero = 0.0; + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(zero, 10, 2), 12); + line_aux += std::string(7, ' '); + line_aux += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GAUT", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GAUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A0_6, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A1_6, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.t0t_6), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WNot_6), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GLUT", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GLUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(glonass_gnav_utc_model.d_tau_c, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(0.0, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(0.0), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if (line_str.find("LEAP SECONDS", 59) != std::string::npos) + { + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.Delta_tLS_6), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.Delta_tLSF_6), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WN_LSF_6), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.DN_6), 6); + line_aux += std::string(36, ' '); + line_aux += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + data.push_back(line_aux); + } + else if (line_str.find("END OF HEADER", 59) != std::string::npos) + { + data.push_back(line_str); + no_more_finds = true; + } + else + { + data.push_back(line_str); + } + + } + else + { + data.push_back(line_str); + } + } + + out.close(); + out.open(navMixfilename, std::ios::out | std::ios::trunc); + out.seekp(0); + for (int i = 0; i < (int) data.size() - 1; i++) + { + out << data[i] << std::endl; + } + out.close(); + out.open(navMixfilename, std::ios::out | std::ios::app); + out.seekp(pos); + std::cout << "The RINEX Navigation file header has been updated with UTC and IONO info." << std::endl; +} + + void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& eph_map) { std::string line; @@ -2622,7 +2818,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond, glonass_gnav_ephemeris_iter->second.d_Toc); + boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_GLONASS_time(glonass_gnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second.d_t_k); std::string timestring = boost::posix_time::to_iso_string(p_utc_time); std::string month (timestring, 4, 2); std::string day (timestring, 6, 2); @@ -2631,7 +2827,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map(gps_ephemeris_iter->second.i_satellite_PRN), 2); + line += Rinex_Printer::rightJustify(boost::lexical_cast(glonass_gnav_ephemeris_iter->second.i_satellite_PRN), 2); line += std::string(1, ' '); std::string year (timestring, 2, 2); line += year; @@ -2718,7 +2914,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.d_gamma_n, 18, 2); line += std::string(1, ' '); //TODO need to define this here. what is nd - line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_t_k + nd*86400, 18, 2); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_t_k + p_utc_time.date().day()*86400, 18, 2); } Rinex_Printer::lengthCheck(line); out << line << std::endl; @@ -2766,7 +2962,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.d_VYn, 18, 2); line += std::string(1, ' '); - line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_ZYn, 18, 2); + line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.d_AYn, 18, 2); line += std::string(1, ' '); line += Rinex_Printer::doub2for(glonass_gnav_ephemeris_iter->second.i_satellite_freq_channel, 18, 2); if (version == 2) @@ -2824,7 +3020,16 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& galileo_eph_map, const std::map& glonass_gnav_eph_map) +{ + version = 3; + stringVersion = "3.02"; + Rinex_Printer::log_rinex_nav(out, galileo_eph_map); + Rinex_Printer::log_rinex_nav(out, glonass_gnav_eph_map); +} + + +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands) { std::string line; std::map::const_iterator glonass_gnav_ephemeris_iter; @@ -3054,7 +3259,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem // -------- TIME OF FIRST OBS line.clear(); - boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph,d_TOW_first_observation); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GLONASS_time(eph,d_TOW_first_observation); std::string timestring = boost::posix_time::to_iso_string(p_gps_time); std::string year (timestring, 0, 4); std::string month (timestring, 4, 2); @@ -3131,6 +3336,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double d_TOW_first_observation, const std::string glonass_bands) { + if(glonass_gnav_eph.d_m){} // avoid warning, not needed std::string line; version = 3; @@ -3305,7 +3511,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps number_of_observations_glo = number_of_observations_glo + 4; } signal_ = "2C"; - std::size_t found_2C = galileo_bands.find(signal_); + std::size_t found_2C = glonass_bands.find(signal_); if(found_2C != std::string::npos) { number_of_observations_glo = number_of_observations_glo + 4; @@ -3314,7 +3520,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps line.clear(); line += satelliteSystem["GLONASS"]; line += std::string(2, ' '); - line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_glO), 3); + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_glo), 3); if(found_1C != std::string::npos) { @@ -3393,6 +3599,321 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps } +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Galileo_Ephemeris& galileo_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double d_TOW_first_observation, const std::string galileo_bands, const std::string glonass_bands) +{ + if(glonass_gnav_eph.d_m){} // avoid warning, not needed + std::string line; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += "3.02"; + line += std::string(11, ' '); + line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); + line += satelliteSystem["Mixed"]; + line += std::string(19, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::leftJustify("G = GPS R = GLONASS E = GALILEO S = GEO M = MIXED", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 3 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("MIXED (GALILEO/GLONASS) OBSERVATION DATA FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER NAME + line.clear(); + line += Rinex_Printer::leftJustify("DEFAULT MARKER NAME", 60); // put a flag or a property, + line += Rinex_Printer::leftJustify("MARKER NAME", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER TYPE + //line.clear(); + //line += Rinex_Printer::leftJustify("NON_GEODETIC", 20); // put a flag or a property + //line += std::string(40, ' '); + //line += Rinex_Printer::leftJustify("MARKER TYPE", 20); + //Rinex_Printer::lengthCheck(line); + //out << line << std::endl; + + // -------- Line OBSERVER / AGENCY + line.clear(); + std::string username; + char c_username[20] = {0}; + int nGet = getlogin_r(c_username, sizeof(c_username) - 1); + if (nGet == 0) + { + username = c_username; + } + else + { + username = "UNKNOWN USER"; + } + line += leftJustify(username, 20); + line += Rinex_Printer::leftJustify("CTTC", 40); // add flag and property + line += Rinex_Printer::leftJustify("OBSERVER / AGENCY", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line REC / TYPE VERS + line.clear(); + line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property + line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property + //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property + if(gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); + line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); + line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); + lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA TYPE + line.clear(); + line += Rinex_Printer::leftJustify("Antenna number", 20); // add flag and property + line += Rinex_Printer::leftJustify("Antenna type", 20); // add flag and property + line += std::string(20, ' '); + line += Rinex_Printer::leftJustify("ANT # / TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- APPROX POSITION (optional for moving platforms) + // put here real data! + double antena_x = 0.0; + double antena_y = 0.0; + double antena_z = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_x, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_y, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_z, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("APPROX POSITION XYZ", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h = 0.0; + double antena_e = 0.0; + double antena_n = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_h, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_e, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("ANTENNA: DELTA H/E/N", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- SYS / OBS TYPES + line.clear(); + unsigned int number_of_observations_gal = 0; + std::string signal_("1B"); + std::size_t found_1B = galileo_bands.find(signal_); + if(found_1B != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + signal_ = "5X"; + std::size_t found_5X = galileo_bands.find(signal_); + if(found_5X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + + line.clear(); + signal_ = "7X"; + std::size_t found_7X = galileo_bands.find(signal_); + if(found_7X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + + + line += satelliteSystem["Galileo"]; + line += std::string(2, ' '); + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_gal), 3); + + if(found_1B != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E1_B"]; + } + + if(found_5X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5a_IQ"]; + } + + if(found_7X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5b_IQ"]; + } + + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + line.clear(); + unsigned int number_of_observations_glo = 0; + signal_ = "1C"; + std::size_t found_1C = glonass_bands.find(signal_); + if(found_1C != std::string::npos) + { + number_of_observations_glo = number_of_observations_glo + 4; + } + signal_ = "2C"; + std::size_t found_2C = glonass_bands.find(signal_); + if(found_2C != std::string::npos) + { + number_of_observations_glo = number_of_observations_glo + 4; + } + + line += satelliteSystem["GLONASS"]; + line += std::string(2, ' '); + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_glo), 3); + + if(found_1C != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_L1_CA"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GLONASS_L1_CA"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GLONASS_L1_CA"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GLONASS_L1_CA"]; + } + + if(found_2C != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GLONASS_L2_CA"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GLONASS_L2_CA"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GLONASS_L2_CA"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GLONASS_L2_CA"]; + } + + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Signal Strength units + line.clear(); + line += Rinex_Printer::leftJustify("DBHZ", 20); + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("SIGNAL STRENGTH UNIT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- TIME OF FIRST OBS + line.clear(); + boost::posix_time::ptime p_galileo_time = Rinex_Printer::compute_Galileo_time(galileo_eph, d_TOW_first_observation); + std::string timestring=boost::posix_time::to_iso_string(p_galileo_time); + std::string year (timestring, 0, 4); + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + double galileo_t = d_TOW_first_observation; + double seconds = fmod(galileo_t, 60); + line += Rinex_Printer::rightJustify(year, 6); + line += Rinex_Printer::rightJustify(month, 6); + line += Rinex_Printer::rightJustify(day, 6); + line += Rinex_Printer::rightJustify(hour, 6); + line += Rinex_Printer::rightJustify(minutes, 6); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); + line += Rinex_Printer::rightJustify(std::string("Galileo"), 8); + line += std::string(9, ' '); + line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- end of header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const double d_TOW_first_observation) { std::string line; @@ -4638,6 +5159,14 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps out << line << std::endl; } +void Rinex_Printer::update_obs_header(std::fstream& out, const Glonass_Gnav_Utc_Model& utc_model) +{ + if(utc_model.d_N_4) + { + + } +} + void Rinex_Printer::update_obs_header(std::fstream& out, const Gps_Utc_Model& utc_model) { @@ -4841,16 +5370,16 @@ void Rinex_Printer::update_obs_header(std::fstream& out, const Galileo_Utc_Model } -void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonassps_Gnav_Ephemeris& eph, const double obs_time, const std::map& observables) +void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, const double obs_time, const std::map& observables, const std::string glonass_band) { // RINEX observations timestamps are GPS timestamps. std::string line; - boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph, obs_time); - std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + boost::posix_time::ptime p_glonass_time = Rinex_Printer::compute_GLONASS_time(eph, obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_glonass_time); //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); //double gps_t = eph.sv_clock_correction(obs_time); - double gps_t = obs_time; + double glonass_t = obs_time; std::string month (timestring, 4, 2); std::string day (timestring, 6, 2); @@ -4888,7 +5417,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonassps_Gnav_Epheme line += std::string(1, ' '); line += minutes; line += std::string(1, ' '); - double second_ = fmod(gps_t, 60); + double second_ = fmod(glonass_t, 60); if (second_ < 10) { line += std::string(1, ' '); @@ -4992,7 +5521,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonassps_Gnav_Epheme line += minutes; line += std::string(1, ' '); - double seconds = fmod(gps_t, 60); + double seconds = fmod(glonass_t, 60); // Add extra 0 if seconds are < 10 if (seconds < 10) { @@ -5084,6 +5613,473 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonassps_Gnav_Epheme } +void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double gps_obs_time, const std::map& observables) +{ + if(glonass_gnav_eph.d_m){} // avoid warning, not needed + std::string line; + + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(gps_eph, gps_obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); + //double gps_t = eph.sv_clock_correction(obs_time); + double gps_t = gps_obs_time; + + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + + std::string year (timestring, 0, 4); + line += std::string(1, '>'); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + + line += std::string(1, ' '); + double seconds = fmod(gps_t, 60); + // Add extra 0 if seconds are < 10 + if (seconds < 10) + { + line +=std::string(1, '0'); + } + line += Rinex_Printer::asString(seconds, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + + //Number of satellites observed in current epoch + + //Get maps with observations + std::map observablesG1C; + std::map observablesR1C; + std::map observablesR2C; + std::map::const_iterator observables_iter; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesR1C.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesR2C.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if((system_.compare("G") == 0) && (sig_.compare("1C") == 0)) + { + observablesG1C.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + std::multimap total_glo_map; + std::set available_glo_prns; + std::set::iterator it; + for(observables_iter = observablesR1C.begin(); + observables_iter != observablesR1C.end(); + observables_iter++) + { + unsigned int prn_ = observables_iter->second.PRN; + total_glo_map.insert(std::pair(prn_, observables_iter->second)); + it = available_glo_prns.find(prn_); + if(it == available_glo_prns.end()) + { + available_glo_prns.insert(prn_); + } + } + + for(observables_iter = observablesR2C.begin(); + observables_iter != observablesR2C.end(); + observables_iter++) + { + unsigned int prn_ = observables_iter->second.PRN; + total_glo_map.insert(std::pair(prn_, observables_iter->second)); + it = available_glo_prns.find(prn_); + if(it == available_glo_prns.end()) + { + available_glo_prns.insert(prn_); + } + } + + int numGloSatellitesObserved = available_glo_prns.size(); + int numGpsSatellitesObserved = observablesG1C.size(); + int numSatellitesObserved = numGloSatellitesObserved + numGpsSatellitesObserved; + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + std::string s; + std::string lineObs; + for(observables_iter = observablesG1C.begin(); + observables_iter != observablesG1C.end(); + observables_iter++) + { + lineObs.clear(); + + s.assign(1, observables_iter->second.System); + if(s.compare("G") == 0) lineObs += satelliteSystem["GPS"]; + if(s.compare("R") == 0) lineObs += satelliteSystem["GLONASS"]; // should not happen + if (static_cast(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(observables_iter->second.PRN)); + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(observables_iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // PHASE + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads/GPS_TWO_PI, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } + + std::pair ::iterator, std::multimap::iterator> ret; + for(it = available_glo_prns.begin(); + it != available_glo_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["GLONASS"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_glo_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } +} + + +void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& galileo_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double galileo_obs_time, const std::map& observables) +{ + if(glonass_gnav_eph.d_m){} // avoid warning, not needed + std::string line; + + boost::posix_time::ptime p_galileo_time = Rinex_Printer::compute_Galileo_time(galileo_eph, galileo_obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_galileo_time); + //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); + //double gps_t = eph.sv_clock_correction(obs_time); + double galileo_t = galileo_obs_time; + + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + + std::string year (timestring, 0, 4); + line += std::string(1, '>'); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + + line += std::string(1, ' '); + double seconds = fmod(galileo_t, 60); + // Add extra 0 if seconds are < 10 + if (seconds < 10) + { + line +=std::string(1, '0'); + } + line += Rinex_Printer::asString(seconds, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + + //Number of satellites observed in current epoch + + //Get maps with observations + std::map observablesE1B; + std::map observablesR1C; + std::map observablesR2C; + std::map::const_iterator observables_iter; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesR1C.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if((system_.compare("R") == 0) && (sig_.compare("2C") == 0)) + { + observablesR2C.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if((system_.compare("E") == 0) && (sig_.compare("1B") == 0)) + { + observablesE1B.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + std::multimap total_glo_map; + std::set available_glo_prns; + std::set::iterator it; + for(observables_iter = observablesR1C.begin(); + observables_iter != observablesR1C.end(); + observables_iter++) + { + unsigned int prn_ = observables_iter->second.PRN; + total_glo_map.insert(std::pair(prn_, observables_iter->second)); + it = available_glo_prns.find(prn_); + if(it == available_glo_prns.end()) + { + available_glo_prns.insert(prn_); + } + } + for(observables_iter = observablesR2C.begin(); + observables_iter != observablesR2C.end(); + observables_iter++) + { + unsigned int prn_ = observables_iter->second.PRN; + total_glo_map.insert(std::pair(prn_, observables_iter->second)); + it = available_glo_prns.find(prn_); + if(it == available_glo_prns.end()) + { + available_glo_prns.insert(prn_); + } + } + + int numGloSatellitesObserved = available_glo_prns.size(); + int numGalSatellitesObserved = observablesE1B.size(); + int numSatellitesObserved = numGalSatellitesObserved + numGloSatellitesObserved; + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + std::string s; + std::string lineObs; + for(observables_iter = observablesE1B.begin(); + observables_iter != observablesE1B.end(); + observables_iter++) + { + lineObs.clear(); + + s.assign(1, observables_iter->second.System); + if(s.compare("E") == 0) lineObs += satelliteSystem["Galileo"]; + if(s.compare("R") == 0) lineObs += satelliteSystem["GLONASS"]; // should not happen + if (static_cast(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(observables_iter->second.PRN)); + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(observables_iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // PHASE + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads/GPS_TWO_PI, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } + + std::pair ::iterator, std::multimap::iterator> ret; + for(it = available_glo_prns.begin(); + it != available_glo_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["Galileo"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_glo_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GLONASS_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // GLONASS SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } +} + + void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, const double obs_time, const std::map& observables) { // RINEX observations timestamps are GPS timestamps. @@ -6344,12 +7340,7 @@ boost::posix_time::ptime Rinex_Printer::compute_Galileo_time(const Galileo_Ephem boost::posix_time::ptime Rinex_Printer::compute_GLONASS_time(const Glonass_Gnav_Ephemeris& eph, const double obs_time) { - // The RINEX v2.11 v3.00 format uses Galileo time for the observations epoch, not UTC time, thus, no leap seconds needed here. - // (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf) - // --??? No time correction here, since it will be done in the RINEX processor - double glonass_t = obs_time; - boost::posix_time::time_duration t = boost::posix_time::millisec((galileo_t + 604800 * static_cast(eph.WN_5)) * 1000); // - boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); + boost::posix_time::ptime p_time = eph.compute_GLONASS_time(obs_time); return p_time; } /* diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 7264a32d2..901fd0a60 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -124,7 +124,7 @@ public: void rinex_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac & galileo_almanac, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); /*! - * \brief Generates the Mixed (GLONASS L1, L2/GPS L1 C/A) Navigation Data header + * \brief Generates the Mixed (GPS L1 C/A/GLONASS L1, L2) Navigation Data header */ void rinex_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc_model, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); @@ -166,7 +166,7 @@ public: /*! * \brief Generates the Mixed (Galileo/GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". */ - void rinex_obs_header(std::fstream & out, const Galileo_Ephemeris & galileo_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1C"); + void rinex_obs_header(std::fstream & out, const Galileo_Ephemeris & galileo_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B", const std::string glo_bands = "1C"); /*! * \brief Generates the SBAS raw data header @@ -218,6 +218,21 @@ public: */ void log_rinex_nav(std::fstream & out, const std::map & gps_eph_map, const std::map & galileo_eph_map); + /*! + * \brief Writes data from the GLONASS GNAV navigation message into the RINEX file + */ + void log_rinex_nav(std::fstream & out, const std::map & eph_map); + + /*! + * \brief Writes data from the Mixed (GPS/GLONASS GNAV) navigation message into the RINEX file + */ + void log_rinex_nav(std::fstream & out, const std::map & gps_eph_map, const std::map & glonass_gnav_eph_map); + + /*! + * \brief Writes data from the Mixed (Galileo/ GLONASS GNAV) navigation message into the RINEX file + */ + void log_rinex_nav(std::fstream & out, const std::map & galileo_eph_map, const std::map & glonass_gnav_eph_map); + /*! * \brief Writes GPS L1 observables into the RINEX file */ @@ -278,7 +293,7 @@ public: void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); - void update_nav_header(std::fstream & out, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); + void update_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void update_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index b46fe7576..b803a6c45 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -126,6 +126,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ obsd_t obs_data[MAXOBS]; eph_t eph_data[MAXOBS]; + geph_t geph_data[MAXOBS]; for(gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); @@ -441,5 +442,5 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } } } - return this->is_valid_position(); -} + return b_valid_position; + } diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index a2374a67d..f7da4d8df 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -68,53 +68,49 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { geph_t rtklib_sat = {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, 0.0}, 0.0, 0.0, 0.0}; + gtime_t t_utc; + struct tm utcinfo; - int iode; /* IODE (0-6 bit of tb field) */ - int frq; /* satellite frequency number */ - int svh,sva,age; /* satellite health, accuracy, age of operation */ - gtime_t toe; /* epoch of epherides (gpst) */ - gtime_t tof; /* message frame time (gpst) */ - double pos[3]; /* satellite position (ecef) (m) */ - double vel[3]; /* satellite velocity (ecef) (m/s) */ - double acc[3]; /* satellite acceleration (ecef) (m/s^2) */ - double taun,gamn; /* SV clock bias (s)/relative freq bias */ - double dtaun; + rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number; /* satellite number */ + rtklib_sat.iode = glonass_gnav_eph.d_t_b; /* IODE (0-6 bit of tb field) */ + rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */ + rtklib_sat.svh = glonass_gnav_eph.d_l_n; /* satellite health*/ + rtklib_sat.sva = glonass_gnav_eph.d_F_T; /* satellite accuracy*/ + rtklib_sat.age = glonass_gnav_eph.d_E_n; /* satellite age*/ + rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn*1000; /* satellite position (ecef) (m) */ + rtklib_sat.pos[1] = glonass_gnav_eph.d_Yn*1000; /* satellite position (ecef) (m) */ + rtklib_sat.pos[2] = glonass_gnav_eph.d_Zn*1000; /* satellite position (ecef) (m) */ + rtklib_sat.vel[0] = glonass_gnav_eph.d_VXn*1000; /* satellite velocity (ecef) (m/s) */ + rtklib_sat.vel[1] = glonass_gnav_eph.d_VYn*1000; /* satellite velocity (ecef) (m/s) */ + rtklib_sat.vel[2] = glonass_gnav_eph.d_VZn*1000; /* satellite velocity (ecef) (m/s) */ + rtklib_sat.acc[0] = glonass_gnav_eph.d_AXn*1000; /* satellite acceleration (ecef) (m/s^2) */ + rtklib_sat.acc[1] = glonass_gnav_eph.d_AYn*1000; /* satellite acceleration (ecef) (m/s^2) */ + rtklib_sat.acc[2] = glonass_gnav_eph.d_AZn*1000; /* satellite acceleration (ecef) (m/s^2) */ + rtklib_sat.taun = glonass_gnav_eph.d_tau_n; /* SV clock bias (s) */ + rtklib_sat.gamn = glonass_gnav_eph.d_gamma_n; /* SV relative freq bias */ + rtklib_sat.age = glonass_gnav_eph.d_Delta_tau_n; /* delay between L1 and L2 (s) */ - rtklib_sat.sat = glonass_gnav_eph.i_satellite_PRN; /* satellite number */ - rtklib_sat.iode = glonass_gnav_eph.d_sqrt_A; - rtklib_sat.frq = glonass_gnav_eph.; - rtklib_sat.svh = glonass_gnav_eph.; - rtklib_sat.sva = glonass_gnav_eph. - rtklib_sat.age = glonass_gnav_eph. + /* TODO: adjustment for week handover */ + utcinfo.tm_mon = 0; + utcinfo.tm_mday = glonass_gnav_eph.d_D4Y; + utcinfo.tm_year = glonass_gnav_eph.d_yr - 1900; + utcinfo.tm_hour = 0; + utcinfo.tm_min = 0; + utcinfo.tm_sec = glonass_gnav_eph.d_t_b; + t_utc.time = mktime(&utcinfo); + t_utc.sec = 0.0; + rtklib_sat.toe = utc2gpst(t_utc); /* epoch of epherides (gpst) */ - rtklib_sat.toe = gpst2time(rtklib_sat.week, gps_eph.d_Toc); - rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_eph.d_TOW); + utcinfo.tm_mon = 0; + utcinfo.tm_mday = glonass_gnav_eph.d_D4Y; + utcinfo.tm_year = glonass_gnav_eph.d_yr - 1900; + utcinfo.tm_hour = 0; + utcinfo.tm_min = 0; + utcinfo.tm_sec = glonass_gnav_eph.d_t_k; + t_utc.time = mktime(&utcinfo); + t_utc.sec = 0.0; + rtklib_sat.tof = utc2gpst(t_utc); /* message frame time (gpst) */ - rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn; - rtklib_sat.pos[1] = glonass_gnav_eph.d_Yn; - rtklib_sat.pos[2] = glonass_gnav_eph.d_Zn; - - rtklib_sat.vel[0] = glonass_gnav_eph.d_VXn; - rtklib_sat.vel[1] = glonass_gnav_eph.d_VYn; - rtklib_sat.vel[2] = glonass_gnav_eph.d_VZn; - - rtklib_sat.acc[0] = glonass_gnav_eph.d_AXn; - rtklib_sat.acc[1] = glonass_gnav_eph.d_AYn; - rtklib_sat.acc[2] = glonass_gnav_eph.d_AZn; - - rtklib_sat.taun = glonass_gnav_eph.d_tau_n; /* SV clock bias (s) */ - rtklib_sat.gann = glonass_gnav_eph.d_gamma_n; /* SV relative freq bias */ - rtklib_sat.gann = glonass_gnav_eph.d_gamma_n; - - /* adjustment for week handover */ - double tow, toc; - tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); - toc = time2gpst(rtklib_sat.toc, NULL); - if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} - else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} - rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); - rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); - rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); return rtklib_sat; } diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 33b21b18f..be05cc58a 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -103,8 +103,8 @@ const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame in the GNAV message 15 strings*(85 data bits + 30 time mark bits)[bits] const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] const int GLONASS_GNAV_FRAME_MS = 30000; //!< Subframe duration [seconds] -const int GLONASS_GNAV_STRING_BITS = 115; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] -const int GLONASS_GNAV_HAMMING_CODE_BITS = 115; //!< Number of bits in hamming code sequence of GNAV message +const int GLONASS_GNAV_STRING_BITS = 85; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] +const int GLONASS_GNAV_HAMMING_CODE_BITS = 8; //!< Number of bits in hamming code sequence of GNAV message const std::vector GLONASS_GNAV_CRC_I_INDEX {9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84}; const std::vector GLONASS_GNAV_CRC_J_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; @@ -124,7 +124,9 @@ const std::vector> STRING_ID({{2,4}}); const std::vector> KX({{78,8}}); //STRING 1 const std::vector> P1({{8,2}}); -const std::vector> T_K({{10,12}}); +const std::vector> T_K_HR({{10,5}}); +const std::vector> T_K_MIN({{10,6}}); +const std::vector> T_K_SEC({{10,1}}); const std::vector> X_N_DOT ({{22,24}}); const std::vector> X_N_DOT_DOT ({{46,5}}); const std::vector> X_N({{51,27}}); diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index cdde02086..002324e94 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -53,6 +53,7 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() d_KP = 0.0; d_tau_n_A = 0.0; d_C_n = 0.0; + d_l_n = 0.0; } void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 7bbf9fe41..1e1682e66 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -62,7 +62,8 @@ public: double d_M_n_A; //!< Type of satellite n_A [dimensionless] double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless] double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] - double d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + double d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] // satellite positions double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. @@ -109,6 +110,7 @@ public: archive & make_nvp("d_KP", d_KP); archive & make_nvp("d_tau_n_A", d_tau_n_A); archive & make_nvp("d_C_n", d_C_n); + archive & make_nvp("d_l_n", d_l_n); } void satellite_position(double N_i, double t_i); diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index 7f48c1051..1127751fe 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -34,6 +34,8 @@ #include "glonass_gnav_ephemeris.h" #include +#include + #include "GLONASS_L1_CA.h" #include "gnss_satellite.h" @@ -77,6 +79,16 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() } +boost::posix_time::ptime Glonass_Gnav_Ephemeris::compute_GLONASS_time(const double offset_time) const +{ + boost::posix_time::time_duration t(0, 0, offset_time); + boost::gregorian::date d(d_yr, 1, d_N_T); + boost::posix_time::ptime glonass_time(d, t); + + return glonass_time; +} + + void Glonass_Gnav_Ephemeris::gravitational_perturbations() { double T = 0.0; diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 6348270a8..cd527717e 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -37,6 +37,7 @@ #include #include "boost/assign.hpp" #include +#include @@ -59,62 +60,68 @@ private: void gravitational_perturbations(); - double d_Jx_moon; //!< Moon gravitational perturbation - double d_Jy_moon; //!< Moon gravitational perturbation - double d_Jz_moon; //!< Moon gravitational perturbation + double d_Jx_moon; //!< Moon gravitational perturbation + double d_Jy_moon; //!< Moon gravitational perturbation + double d_Jz_moon; //!< Moon gravitational perturbation - double d_Jx_sun; //!< Sun gravitational perturbation - double d_Jy_sun; //!< Sun gravitational perturbation - double d_Jz_sun; //!< Sun gravitational perturbation + double d_Jx_sun; //!< Sun gravitational perturbation + double d_Jy_sun; //!< Sun gravitational perturbation + double d_Jz_sun; //!< Sun gravitational perturbation public: - int i_satellite_freq_channel; // SV Frequency Channel Number - double d_m; //!< String number within frame [dimensionless] - double d_t_k; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] - double d_t_b; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] - double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] - double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] - double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + 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] + double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] + double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] + double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + double d_Xn; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + double d_Yn; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + double d_Zn; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + double d_VXn; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + double d_VYn; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + double d_VZn; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + double d_AXn; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_AYn; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_AZn; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + double d_B_n; //!< Health flag [dimensionless] + double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] + double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] + double d_F_T; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] + double d_n; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation + double d_Delta_tau_n; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] + double d_E_n; //!< Characterises "age" of a current information [days] + double d_P_1; //!< Flag of the immediate data updating [minutes] + double d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + double d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - // satellite positions - double d_Xn; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - double d_Yn; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - double d_Zn; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity - double d_VXn; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - double d_VYn; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - double d_VZn; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration - double d_AXn; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_AYn; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_AZn; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + // Inmediate deliverables of ephemris information + int i_satellite_freq_channel; //!< SV Frequency Channel Number + unsigned int i_satellite_PRN; //!< SV PRN NUMBER + unsigned int i_satellite_slot_number; //!< SV PRN NUMBER + double d_TOD; //!< Time of Day of the ephemeris set based in start of frame [s] + double d_D4Y; //!< Day of Year after latest leap year (4 year interval) + double d_yr; //!< Current year + double d_satClkDrift; //!< GLONASS clock error + double d_dtr; //!< relativistic clock correction term + double d_iode; //!< Issue of data, ephemeris (Bit 0-6 of tb) + double d_tau_c; + double d_TOW; // tow of the start of frame + double d_WN; // week number of the start of frame - double d_B_n; //!< Health flag [dimensionless] - double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] - double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] - double d_F_T; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] - double d_n; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation - double d_Delta_tau_n; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] - double d_E_n; //!< Characterises "age" of a current information [days] - double d_P_1; //!< Flag of the immediate data updating. - double d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] - double d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] - double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + // Need to add a way to compute the GPS week number and GPS TIME OF WEEK from GLONASS ephemeris - // clock terms derived from ephemeris data - double d_satClkDrift; //!< GPS clock error - double d_dtr; //!< relativistic clock correction term - - // satellite positions + // satellite positions after RK4 Integration double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity + // Satellite velocity after RK4 Integration double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration + // Satellite acceleration after RK4 Integration double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] @@ -136,6 +143,15 @@ public: archive & make_nvp("d_M", d_M); //!< Type of satellite transmitting navigation signal [dimensionless] archive & make_nvp("d_gamma_n", d_gamma_n); //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] archive & make_nvp("d_tau_n", d_tau_n); //!< Correction to the nth satellite time (tn) relative to GLONASS time (te) + archive & make_nvp("d_Xn", d_Xn); //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + archive & make_nvp("d_Yn", d_Yn); //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + archive & make_nvp("d_Zn", d_Zn); //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + archive & make_nvp("d_VXn", d_VXn); //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + archive & make_nvp("d_VYn", d_VYn); //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + archive & make_nvp("d_VZn", d_VZn); //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + archive & make_nvp("d_AXn", d_AXn); //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + archive & make_nvp("d_AYn", d_AYn); //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + archive & make_nvp("d_AZn", d_AZn); //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] archive & make_nvp("d_B_n", d_B_n); //!< Health flag [dimensionless] archive & make_nvp("d_P", d_P); //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] archive & make_nvp("d_N_T", d_N_T); //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] @@ -172,7 +188,7 @@ public: * \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 sv_clock_drift(double transmitTime, double timeCorrUTC); /*! * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction @@ -181,6 +197,12 @@ public: double sv_clock_relativistic_term(double transmitTime); + /*! + * \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; + /*! * Default constructor */ diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 96608b736..ee76d31bf 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -104,7 +104,7 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() } -bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset bits) +bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset bits) { int sum_bits; int sum_hamming; @@ -118,7 +118,7 @@ bool Glonass_Gnav_Navigation_Message::_CRC_test(std::bitset data = std::bitset(bits.to_string(), 0, 77); - std::bitset hamming_code = std::bitset(bits.to_string(), 77, 8); + std::bitset hamming_code = std::bitset(bits.to_string(), 77, 8); std::istringstream dsb = std::istringstream( data.to_string() ); std::istringstream hcb = std::istringstream( hamming_code.to_string() ); @@ -345,13 +345,14 @@ unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int sate int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) { int string_ID = 0; - int frame_ID = 0; + int J = 0; + frame_ID = 0; // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE std::bitset string_bits = std::bitset(std::string(frame_string)); string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); - _CRC_test(string_bits); + CRC_test(string_bits); // Decode all 15 string messages switch (string_ID) @@ -359,11 +360,17 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 1: //--- It is string 1 ----------------------------------------------- gnav_ephemeris.d_P_1 = static_cast(read_navigation_unsigned(string_bits, P1)); - gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K)); - gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)); - gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)); - gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)); + gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K_HR)) * 3600 + + static_cast(read_navigation_unsigned(string_bits, T_K_MIN)) * 60 + + static_cast(read_navigation_unsigned(string_bits, T_K_SEC)) * 30; + gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)) * 2e-20; + gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)) * 2e-30; + gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * 2e-11; + if(gnav_ephemeris.d_P_1 != 0) + { + gnav_ephemeris.d_P_1 = (gnav_ephemeris.d_P_1 + 1)*15; + } flag_ephemeris_str_1 = true; break; @@ -372,11 +379,12 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) //--- It is string 2 ----------------------------------------------- gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); - gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B)); - gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT)); - gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)); - gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)); + gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*15*60; + gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* 2e-20; + gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * 2e-30; + gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)) * 2e-11; + gnav_ephemeris.d_iode = read_navigation_unsigned(string_bits, T_B); flag_ephemeris_str_2 = true; break; @@ -384,22 +392,21 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 3: // --- It is string 3 ---------------------------------------------- gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); - gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)); + gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * 2e-40; gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); - gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)); - gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)); - gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)); + gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)) * 2e-20; + gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)) * 2e-30; + gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)) * 2e-11; flag_ephemeris_str_3 = true; break; case 4: - // --- It is subframe 4 -------------------------------------------- - // TODO signed vs unsigned reading from datasheet - gnav_ephemeris.d_tau_n = static_cast(read_navigation_unsigned(string_bits, TAU_N)); - gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)); + // --- It is string 4 ---------------------------------------------- + gnav_ephemeris.d_tau_n = static_cast(read_navigation_signed(string_bits, TAU_N)) * 2e-30; + gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)) * 2e-30; gnav_ephemeris.d_E_n = static_cast(read_navigation_unsigned(string_bits, E_N)); gnav_ephemeris.d_P_4 = static_cast(read_navigation_unsigned(string_bits, P4)); gnav_ephemeris.d_F_T = static_cast(read_navigation_unsigned(string_bits, F_T)); @@ -407,33 +414,59 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_n = static_cast(read_navigation_unsigned(string_bits, N)); gnav_ephemeris.d_M = static_cast(read_navigation_unsigned(string_bits, M)); + // Fill in ephemeris deliverables in the code + gnav_ephemeris.i_satellite_slot_number = gnav_ephemeris.d_n; + gnav_ephemeris.d_D4Y = gnav_ephemeris.d_N_T; + flag_ephemeris_str_4 = true; break; case 5: // --- It is string 5 ---------------------------------------------- - // TODO signed vs unsigned reading from datasheet gnav_utc_model.d_N_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_utc_model.d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + gnav_utc_model.d_tau_c = static_cast(read_navigation_signed(string_bits, TAU_C)) * 2e-31; gnav_utc_model.d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); - gnav_utc_model.d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + gnav_utc_model.d_tau_gps = static_cast(read_navigation_signed(string_bits, TAU_GPS)) * 2e-30; gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Compute Year and DoY based on Algorithm A3.11 of GLONASS ICD + if(flag_ephemeris_str_4 == true) + { + //Current year number J in the four-year interval is calculated: + if(gnav_ephemeris.d_N_T >= 1 && gnav_ephemeris.d_N_T <= 366) + { + J = 1; + } + else if ( gnav_ephemeris.d_N_T >= 367 && gnav_ephemeris.d_N_T <= 731) + { + J = 2; + } + else if (gnav_ephemeris.d_N_T >= 732 && gnav_ephemeris.d_N_T <= 1096) + { + J = 3; + } + else if (gnav_ephemeris.d_N_T >= 1097 && gnav_ephemeris.d_N_T <= 1461) + { + J = 4; + } + // 2). Current year in common form is calculated by the following formula: + gnav_ephemeris.d_yr = 1996 + 4.0*(gnav_utc_model.d_N_4 - 1.0) + (J - 1.0); + } break; case 6: // --- It is string 6 ---------------------------------------------- - // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; flag_almanac_str_6 = true; @@ -443,14 +476,17 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // --- It is string 7 ---------------------------------------------- if (flag_almanac_str_6 == true) { - // TODO signed vs unsigned reading from datasheet - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + if(i_satellite_slot_number == gnav_ephemeris.i_satellite_slot_number) + { + gnav_ephemeris.i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A; + } flag_almanac_str_7 = true; } @@ -458,15 +494,16 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) break; case 8: // --- It is string 8 ---------------------------------------------- - // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + frame_ID = get_frame_number(i_satellite_slot_number); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; flag_almanac_str_8 = true; @@ -477,94 +514,95 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) if (flag_almanac_str_8 == true) { // TODO signed vs unsigned reading from datasheet - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) -32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_9 = true; } case 10: - // --- It is string 8 ---------------------------------------------- - // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + // --- It is string 10 --------------------------------------------- + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + frame_ID = get_frame_number(i_satellite_slot_number); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; flag_almanac_str_10 = true; break; case 11: - // --- It is string 9 ---------------------------------------------- + // --- It is string 11 --------------------------------------------- if (flag_almanac_str_10 == true) { - // TODO signed vs unsigned reading from datasheet - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_11 = true; } break; case 12: - // --- It is string 8 ---------------------------------------------- - // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + // --- It is string 12 --------------------------------------------- + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + frame_ID = get_frame_number(i_satellite_slot_number); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; flag_almanac_str_12 = true; break; case 13: - // --- It is string 9 ---------------------------------------------- + // --- It is string 13 --------------------------------------------- if (flag_almanac_str_12 == true) { - // TODO signed vs unsigned reading from datasheet - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_13 = true; } case 14: - // --- It is string 8 ---------------------------------------------- - if( frame_number == 5) + // --- It is string 14 --------------------------------------------- + if( frame_ID == 5) { gnav_utc_model.d_B1 = static_cast(read_navigation_unsigned(string_bits, B1)); gnav_utc_model.d_B2 = static_cast(read_navigation_unsigned(string_bits, B2)); } else { - // TODO signed vs unsigned reading from datasheet - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, N_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + frame_ID = get_frame_number(i_satellite_slot_number); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; flag_almanac_str_14 = true; } @@ -574,14 +612,13 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 15: // --- It is string 9 ---------------------------------------------- - if (frame_number != 5 and flag_almanac_str_14 == true ) + if (frame_ID != 5 and flag_almanac_str_14 == true ) { - // TODO signed vs unsigned reading from datasheet - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_unsigned(string_bits, OMEGA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_15 = true; @@ -631,7 +668,6 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a //if all ephemeris pages have the same IOD, then they belong to the same block if ((gnav_ephemeris.d_t_b == 0) ) { - std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; flag_ephemeris_str_1 = false;// clear the flag flag_ephemeris_str_2 = false;// clear the flag flag_ephemeris_str_3 = false;// clear the flag diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 18d9025f2..c93eda14c 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -58,13 +58,10 @@ private: unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); bool read_navigation_bool(std::bitset bits, const std::vector> parameter); - bool _CRC_test(std::bitset bits); - - unsigned int get_frame_number(unsigned int satellite_slot_number); public: bool flag_CRC_test; - unsigned int frame_number; + unsigned int frame_ID; Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information @@ -117,6 +114,10 @@ public: double d_dtr; // Relativistic clock correction term double d_satClkDrift; // Satellite clock drift + bool CRC_test(std::bitset bits); + + unsigned int get_frame_number(unsigned int satellite_slot_number); + /*! * \brief Reset GLONASS GNAV Navigation Information */ @@ -160,7 +161,7 @@ public: /*! * \brief Computes the Coordinated Universal Time (UTC) and returns it in [s] */ - double utc_time(const double gpstime_corrected) const; + double utc_time(const double glonasstime_corrected) const; /*! * Default constructor diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index 51d47f6c3..6aca883f4 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -48,7 +48,7 @@ public: double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] - double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days] + double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year for Almanac data [days] double d_B1; //!< Coefficient to determine DeltaUT1 [s] double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd] diff --git a/src/core/system_parameters/gps_ephemeris.h b/src/core/system_parameters/gps_ephemeris.h index 0a413a927..a08f34400 100644 --- a/src/core/system_parameters/gps_ephemeris.h +++ b/src/core/system_parameters/gps_ephemeris.h @@ -37,6 +37,7 @@ #include #include "boost/assign.hpp" #include +#include From 6669482e91250996fba3fad831d1477ae30f5a4b Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 1 Aug 2017 11:23:34 -0700 Subject: [PATCH 29/48] rtcm: Starting coding of RTCM standard Starting to add GLONASS support for RTCM v3 messages while fixing bugs with the RINEX parser. In addition, adding more unit tests in the code --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 56 +- .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 12 +- src/algorithms/PVT/libs/rtcm_printer.cc | 61 +- src/algorithms/PVT/libs/rtcm_printer.h | 62 +- .../glonass_gnav_ephemeris.h | 5 +- .../glonass_gnav_navigation_message.cc | 108 +- src/core/system_parameters/rtcm.cc | 922 +++++++++++++++++- src/core/system_parameters/rtcm.h | 311 +++++- .../glonass_gnav_nav_message_test.cc | 138 +++ 9 files changed, 1567 insertions(+), 108 deletions(-) create mode 100644 src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 8e1e9fbe0..9a4c0b899 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -138,38 +138,48 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, * 20 | GPS L5 + Galileo E5b * 21 | GPS L1 C/A + Galileo E1B + GPS L2C * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + * 23 | GLONASS L1 C/A + * 24 | GLONASS L2 C/A + * 25 | GLONASS L1 C/A + GLONASS L2 C/A + * 26 | GPS L1 C/A + GLONASS L1 C/A + * 27 | Galileo E1B + GLONASS L1 C/A */ int gps_1C_count = configuration->property("Channels_1C.count", 0); int gps_2S_count = configuration->property("Channels_2S.count", 0); int gal_1B_count = configuration->property("Channels_1B.count", 0); int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? int gal_E5b_count = configuration->property("Channels_7X.count", 0); + int glo_1R_count = configuration->property("Channels_1R.count", 0); unsigned int type_of_receiver = 0; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 1; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 2; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 1; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 2; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 4; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 5; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 6; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 4; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 5; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 6; - if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 7; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 7; //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 9; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 10; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 11; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 12; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 9; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 10; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 11; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 12; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 14; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 15; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 14; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 15; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 17; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 18; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 17; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 18; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; - if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 21; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 21; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; - + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0)) type_of_receiver = 23; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0) && (glo_2R_count != 0)) type_of_receiver = 24; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0) && (glo_1R_count != 0)) type_of_receiver = 25; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0)) type_of_receiver = 26; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0)) type_of_receiver = 27; //RTKLIB PVT solver options // Settings 1 int positioning_mode = -1; @@ -192,10 +202,10 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, } int num_bands = 0; - if ((gps_1C_count > 0) || (gal_1B_count > 0)) num_bands = 1; - if (((gps_1C_count > 0) || (gal_1B_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; - if (((gps_1C_count > 0) || (gal_1B_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0)) ) num_bands = 2; - if (((gps_1C_count > 0) || (gal_1B_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) num_bands = 3; + if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) num_bands = 1; + if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; + if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0)) ) num_bands = 2; + if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) num_bands = 3; int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ if( (number_of_frequencies < 1) || (number_of_frequencies > 3) ) { @@ -207,6 +217,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, if( (elevation_mask < 0.0) || (elevation_mask > 90.0) ) { //warn user and set the default + LOG(WARNING) << "Erroneous Elevation Mask. Setting to default value of 15.0 degrees"; elevation_mask = 15.0; } @@ -214,6 +225,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, if( (dynamics_model < 0) || (dynamics_model > 2) ) { //warn user and set the default + LOG(WARNING) << "Erroneous Dynamics Model configuration. Setting to default value of (0:none)"; dynamics_model = 0; } @@ -275,10 +287,12 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int nsys = 0; if ((gps_1C_count > 0) || (gps_2S_count > 0)) nsys += SYS_GPS; if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL; + if ((glo_1R_count > 0)) nsys += SYS_GLO; int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ if( (navigation_system < 1) || (navigation_system > 255) ) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ { //warn user and set the default + LOG(WARNING) << "Erroneous Navigation System. Setting to default value of (0:none)"; navigation_system = nsys; } @@ -305,6 +319,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, if( (integer_ambiguity_resolution_glo < 0) || (integer_ambiguity_resolution_glo > 3) ) { //warn user and set the default + LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for GLONASS . Setting to default value of (1:on)"; integer_ambiguity_resolution_glo = 1; } @@ -312,6 +327,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, if( (integer_ambiguity_resolution_bds < 0) || (integer_ambiguity_resolution_bds > 1) ) { //warn user and set the default + LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for BEIDOU . Setting to default value of (1:on)"; integer_ambiguity_resolution_bds = 1; } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index a188c5809..a46d26180 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -99,10 +99,12 @@ private: bool b_rinex_header_written; bool b_rinex_header_updated; bool b_rtcm_writing_started; - int d_rtcm_MT1045_rate_ms; - int d_rtcm_MT1019_rate_ms; - int d_rtcm_MT1077_rate_ms; - int d_rtcm_MT1097_rate_ms; + int d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris + int d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits) + int d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits) + int d_rtcm_MT1077_rate_ms; //!< The type 7 Multiple Signal Message format for the USA’s GPS system, popular + int d_rtcm_MT1087_rate_ms; //!< GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system + int d_rtcm_MT1097_rate_ms; //!< Galileo MSM7. The type 7 Multiple Signal Message format for Europe’s Galileo system int d_rtcm_MSM_rate_ms; int d_last_status_print_seg; //for status printer @@ -122,8 +124,10 @@ private: double d_rx_time; double last_pvt_display_T_rx_s; double last_RTCM_1019_output_time; + double last_RTCM_1020_output_time; double last_RTCM_1045_output_time; double last_RTCM_1077_output_time; + double last_RTCM_1087_output_time; double last_RTCM_1097_output_time; double last_RTCM_MSM_output_time; double last_RINEX_obs_output_time; diff --git a/src/algorithms/PVT/libs/rtcm_printer.cc b/src/algorithms/PVT/libs/rtcm_printer.cc index aa80318ed..13873251d 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.cc +++ b/src/algorithms/PVT/libs/rtcm_printer.cc @@ -186,6 +186,38 @@ bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNA } +bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables) +{ + std::string m1009 = rtcm->print_MT1009(glonass_gnav_eph, obs_time, observables, station_id); + Rtcm_Printer::Print_Message(m1009); + return true; +} + + +bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables) +{ + std::string m1010 = rtcm->print_MT1010(glonass_gnav_eph, obs_time, observables, station_id); + Rtcm_Printer::Print_Message(m1010); + return true; +} + + +bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables) +{ + std::string m1011 = rtcm->print_MT1011(glonass_gnav_eph, obs_time, observables, station_id); + Rtcm_Printer::Print_Message(m1011); + return true; +} + + +bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables) +{ + std::string m1012 = rtcm->print_MT1012(glonass_gnav_eph, obs_time, observables, station_id); + Rtcm_Printer::Print_Message(m1012); + return true; +} + + bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph) { std::string m1019 = rtcm->print_MT1019(gps_eph); @@ -194,6 +226,14 @@ bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph) } +bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph); + Rtcm_Printer::Print_Message(m1020); + return true; +} + + bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris & gal_eph) { std::string m1045 = rtcm->print_MT1045(gal_eph); @@ -205,6 +245,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris & gal_eph) bool Rtcm_Printer::Print_Rtcm_MSM(unsigned int msm_number, const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int clock_steering_indicator, @@ -216,31 +257,31 @@ bool Rtcm_Printer::Print_Rtcm_MSM(unsigned int msm_number, const Gps_Ephemeris & std::string msm; if(msm_number == 1) { - msm = rtcm->print_MSM_1(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_1(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else if(msm_number == 2) { - msm = rtcm->print_MSM_2(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_2(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else if(msm_number == 3) { - msm = rtcm->print_MSM_3(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_3(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else if(msm_number == 4) { - msm = rtcm->print_MSM_4(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_4(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else if(msm_number == 5) { - msm = rtcm->print_MSM_5(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_5(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else if(msm_number == 6) { - msm = rtcm->print_MSM_6(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_6(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else if(msm_number == 7) { - msm = rtcm->print_MSM_7(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); + msm = rtcm->print_MSM_7(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); } else { @@ -348,3 +389,9 @@ unsigned int Rtcm_Printer::lock_time(const Galileo_Ephemeris& eph, double obs_ti { return rtcm->lock_time(eph, obs_time, gnss_synchro); } + + +unsigned int Rtcm_Printer::lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro) +{ + return rtcm->lock_time(eph, obs_time, gnss_synchro); +} diff --git a/src/algorithms/PVT/libs/rtcm_printer.h b/src/algorithms/PVT/libs/rtcm_printer.h index 0b59001e6..e2622d678 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.h +++ b/src/algorithms/PVT/libs/rtcm_printer.h @@ -59,12 +59,63 @@ public: bool Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map & observables); bool Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map & observables); bool Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map & observables); + /*! + * \brief Prints L1-Only GLONASS RTK Observables + * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred. + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return true or false upon operation success + */ + bool Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); + /*! + * \brief Prints Extended L1-Only GLONASS RTK Observables + * \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases. + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return true or false upon operation success + */ + bool Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); + /*! + * \brief Prints L1&L2 GLONASS RTK Observables + * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return true or false upon operation success + */ + bool Print_Rtcm_MT1011(Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); + /*! + * \brief Prints Extended L1&L2 GLONASS RTK Observables + * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return true or false upon operation success + */ + bool Print_Rtcm_MT1012(Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); + bool Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph); // & observables, unsigned int clock_steering_indicator, @@ -77,6 +128,15 @@ public: unsigned int lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); unsigned int lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); unsigned int lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); + /*! + * \brief Locks time for logging given GLONASS GNAV Broadcast Ephemeris + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return locked time during logging process + */ + unsigned int lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); private: std::string rtcm_filename; // String with the RTCM log filename diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index cd527717e..8735d81cb 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -97,8 +97,9 @@ public: double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - // Inmediate deliverables of ephemris information - int i_satellite_freq_channel; //!< SV Frequency Channel Number + // Inmediate deliverables of ephemeris information + //TODO check how freq channel is managed in gnav message. I think it is a number greater thn 0 + unsigned int i_satellite_freq_channel; //!< SV Frequency Channel Number unsigned int i_satellite_PRN; //!< SV PRN NUMBER unsigned int i_satellite_slot_number; //!< SV PRN NUMBER double d_TOD; //!< Time of Day of the ephemeris set based in start of frame [s] diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index ee76d31bf..1409eaa1d 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -363,9 +363,9 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K_HR)) * 3600 + static_cast(read_navigation_unsigned(string_bits, T_K_MIN)) * 60 + static_cast(read_navigation_unsigned(string_bits, T_K_SEC)) * 30; - gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)) * 2e-20; - gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)) * 2e-30; - gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * 2e-11; + gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)) * TWO_N20; + gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)) * TWO_N30; + gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * TWO_N11; if(gnav_ephemeris.d_P_1 != 0) { @@ -380,9 +380,9 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*15*60; - gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* 2e-20; - gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * 2e-30; - gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)) * 2e-11; + gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* TWO_N20; + gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * TWO_N30; + gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)) * TWO_N11; gnav_ephemeris.d_iode = read_navigation_unsigned(string_bits, T_B); flag_ephemeris_str_2 = true; @@ -392,12 +392,12 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 3: // --- It is string 3 ---------------------------------------------- gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); - gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * 2e-40; + gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * TWO_N30; gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); - gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)) * 2e-20; - gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)) * 2e-30; - gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)) * 2e-11; + gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)) * TWO_N20; + gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)) * TWO_N30; + gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)) * TWO_N11; flag_ephemeris_str_3 = true; @@ -405,8 +405,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 4: // --- It is string 4 ---------------------------------------------- - gnav_ephemeris.d_tau_n = static_cast(read_navigation_signed(string_bits, TAU_N)) * 2e-30; - gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)) * 2e-30; + gnav_ephemeris.d_tau_n = static_cast(read_navigation_signed(string_bits, TAU_N)) * TWO_N30; + gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)) * TWO_N30; gnav_ephemeris.d_E_n = static_cast(read_navigation_unsigned(string_bits, E_N)); gnav_ephemeris.d_P_4 = static_cast(read_navigation_unsigned(string_bits, P4)); gnav_ephemeris.d_F_T = static_cast(read_navigation_unsigned(string_bits, F_T)); @@ -425,9 +425,9 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 5: // --- It is string 5 ---------------------------------------------- gnav_utc_model.d_N_A = static_cast(read_navigation_unsigned(string_bits, N_A)); - gnav_utc_model.d_tau_c = static_cast(read_navigation_signed(string_bits, TAU_C)) * 2e-31; + gnav_utc_model.d_tau_c = static_cast(read_navigation_signed(string_bits, TAU_C)) * TWO_N31; gnav_utc_model.d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); - gnav_utc_model.d_tau_gps = static_cast(read_navigation_signed(string_bits, TAU_GPS)) * 2e-30; + gnav_utc_model.d_tau_gps = static_cast(read_navigation_signed(string_bits, TAU_GPS)) * TWO_N30; gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); // Compute Year and DoY based on Algorithm A3.11 of GLONASS ICD @@ -463,10 +463,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20; flag_almanac_str_6 = true; @@ -476,10 +476,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // --- It is string 7 ---------------------------------------------- if (flag_almanac_str_6 == true) { - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); @@ -500,10 +500,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20; flag_almanac_str_8 = true; @@ -514,10 +514,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) if (flag_almanac_str_8 == true) { // TODO signed vs unsigned reading from datasheet - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) -32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); @@ -531,10 +531,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20; flag_almanac_str_10 = true; @@ -544,10 +544,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // --- It is string 11 --------------------------------------------- if (flag_almanac_str_10 == true) { - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); @@ -562,10 +562,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20; flag_almanac_str_12 = true; @@ -575,10 +575,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // --- It is string 13 --------------------------------------------- if (flag_almanac_str_12 == true) { - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); @@ -599,10 +599,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); - gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * 2e-18; - gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * 2e-20; - gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * 2e-20; + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20; + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20; flag_almanac_str_14 = true; } @@ -614,10 +614,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // --- It is string 9 ---------------------------------------------- if (frame_ID != 5 and flag_almanac_str_14 == true ) { - gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * 2e-15; - gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * 2e-5; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * 2e-9; - gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * 2e-14; + gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15; + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 9999b3671..a621e4e5c 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -1080,6 +1080,410 @@ std::string Rtcm::print_MT1008(unsigned int ref_id, const std::string & antenna_ } +// ******************************************************** +// +// MESSAGE TYPE 1009 (GLONASS L1 Basic RTK Observables) +// +// ******************************************************** +std::bitset<61> Rtcm::get_MT1009_12_header(unsigned int msg_number, double obs_time, const std::map & observables, + unsigned int ref_id, unsigned int smooth_int, bool sync_flag, bool divergence_free) +{ + unsigned int reference_station_id = ref_id; // Max: 4095 + const std::map observables_ = observables; + bool synchronous_GNSS_flag = sync_flag; + bool divergence_free_smoothing_indicator = divergence_free; + unsigned int smoothing_interval = smooth_int; + Rtcm::set_DF002(msg_number); + Rtcm::set_DF003(reference_station_id); + Rtcm::set_DF034(obs_time); + Rtcm::set_DF005(synchronous_GNSS_flag); + Rtcm::set_DF035(observables_); + Rtcm::set_DF036(divergence_free_smoothing_indicator); + Rtcm::set_DF037(smoothing_interval); + + std::string header = DF002.to_string() + + DF003.to_string() + + DF034.to_string() + + DF005.to_string() + + DF035.to_string() + + DF036.to_string() + + DF037.to_string(); + + std::bitset<61> header_msg(header); + return header_msg; +} + + +std::bitset<64> Rtcm::get_MT1009_sat_content(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro) +{ + bool code_indicator = false; // code indicator 0: C/A code 1: P(Y) code direct + Rtcm::set_DF038(gnss_synchro); + Rtcm::set_DF039(code_indicator); + Rtcm::set_DF040(eph.i_satellite_freq_channel); + Rtcm::set_DF041(gnss_synchro); + Rtcm::set_DF042(gnss_synchro); + Rtcm::set_DF043(eph, obs_time, gnss_synchro); + + std::string content = DF038.to_string() + + DF039.to_string() + + DF040.to_string() + + DF041.to_string() + + DF042.to_string() + + DF043.to_string(); + + std::bitset<64> content_msg(content); + return content_msg; +} + + +std::string Rtcm::print_MT1009(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id) +{ + unsigned int ref_id = static_cast(station_id); + unsigned int smooth_int = 0; + bool sync_flag = false; + bool divergence_free = false; + + //Get a map with GLONASS L1 only observations + std::map observablesL1; + std::map::const_iterator observables_iter; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesL1.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + std::bitset<61> header = Rtcm::get_MT1009_12_header(1009, obs_time, observablesL1, ref_id, smooth_int, sync_flag, divergence_free); + std::string data = header.to_string(); + + for(observables_iter = observablesL1.begin(); + observables_iter != observablesL1.end(); + observables_iter++) + { + std::bitset<64> content = Rtcm::get_MT1009_sat_content(glonass_gnav_eph, obs_time, observables_iter->second); + data += content.to_string(); + } + + std::string msg = build_message(data); + if(server_is_running) + { + rtcm_message_queue->push(msg); + } + return msg; +} + + +// ******************************************************** +// +// MESSAGE TYPE 1010 (EXTENDED GLONASS L1 OBSERVATIONS) +// +// ******************************************************** + +std::string Rtcm::print_MT1010(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id) +{ + unsigned int ref_id = static_cast(station_id); + unsigned int smooth_int = 0; + bool sync_flag = false; + bool divergence_free = false; + + //Get a map with GPS L1 only observations + std::map observablesL1; + std::map::const_iterator observables_iter; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesL1.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + std::bitset<61> header = Rtcm::get_MT1009_12_header(1010, obs_time, observablesL1, ref_id, smooth_int, sync_flag, divergence_free); + std::string data = header.to_string(); + + for(observables_iter = observablesL1.begin(); + observables_iter != observablesL1.end(); + observables_iter++) + { + std::bitset<79> content = Rtcm::get_MT1010_sat_content(glonass_gnav_eph, obs_time, observables_iter->second); + data += content.to_string(); + } + + std::string msg = build_message(data); + if(server_is_running) + { + rtcm_message_queue->push(msg); + } + return msg; +} + + +std::bitset<79> Rtcm::get_MT1010_sat_content(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro) +{ + bool code_indicator = false; // code indicator 0: C/A code 1: P(Y) code direct + Rtcm::set_DF038(gnss_synchro); + Rtcm::set_DF039(code_indicator); + Rtcm::set_DF040(eph.i_satellite_freq_channel); + Rtcm::set_DF041(gnss_synchro); + Rtcm::set_DF042(gnss_synchro); + Rtcm::set_DF043(eph, obs_time, gnss_synchro); + Rtcm::set_DF044(gnss_synchro); + Rtcm::set_DF045(gnss_synchro); + + std::string content = DF038.to_string() + + DF039.to_string() + + DF040.to_string() + + DF041.to_string() + + DF042.to_string() + + DF043.to_string() + + DF044.to_string() + + DF045.to_string(); + + std::bitset<79> content_msg(content); + return content_msg; +} + + + +// ******************************************************** +// +// MESSAGE TYPE 1011 (GLONASS L1 & L2 OBSERVATIONS) +// +// ******************************************************** + +std::string Rtcm::print_MT1011(const Glonass_Gnav_Ephemeris & ephL1, const Glonass_Gnav_Ephemeris & ephL2, double obs_time, const std::map & observables, unsigned short station_id) +{ + unsigned int ref_id = static_cast(station_id); + unsigned int smooth_int = 0; + bool sync_flag = false; + bool divergence_free = false; + + //Get maps with GPS L1 and L2 observations + std::map observablesL1; + std::map observablesL2; + std::map::const_iterator observables_iter; + std::map::const_iterator observables_iter2; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesL1.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if((system_.compare("R") == 0) && (sig_.compare("2C") == 0)) + { + observablesL2.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + // Get common observables + std::vector< std::pair< Gnss_Synchro, Gnss_Synchro > > common_observables; + std::vector< std::pair< Gnss_Synchro, Gnss_Synchro > >::const_iterator common_observables_iter; + std::map observablesL1_with_L2; + + for(observables_iter = observablesL1.begin(); + observables_iter != observablesL1.end(); + observables_iter++) + { + unsigned int prn_ = observables_iter->second.PRN; + for(observables_iter2 = observablesL2.begin(); + observables_iter2 != observablesL2.end(); + observables_iter2++) + { + if(observables_iter2->second.PRN == prn_) + { + std::pair p; + Gnss_Synchro pr1 = observables_iter->second; + Gnss_Synchro pr2 = observables_iter2->second; + p = std::make_pair(pr1, pr2); + common_observables.push_back(p); + observablesL1_with_L2.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + } + + std::bitset<61> header = Rtcm::get_MT1009_12_header(1011, obs_time, observablesL1_with_L2, ref_id, smooth_int, sync_flag, divergence_free); + std::string data = header.to_string(); + + for(common_observables_iter = common_observables.begin(); + common_observables_iter != common_observables.end(); + common_observables_iter++) + { + std::bitset<107> content = Rtcm::get_MT1011_sat_content(ephL1, ephL2, obs_time, common_observables_iter->first, common_observables_iter->second); + data += content.to_string(); + } + + std::string msg = build_message(data); + if(server_is_running) + { + rtcm_message_queue->push(msg); + } + return msg; +} + + +std::bitset<107> Rtcm::get_MT1011_sat_content(const Glonass_Gnav_Ephemeris & ephL1, const Glonass_Gnav_Ephemeris & ephL2, double obs_time, const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2) +{ + bool code_indicator = false; // code indicator 0: C/A code 1: P(Y) code direct + Rtcm::set_DF038(gnss_synchroL1); + Rtcm::set_DF039(code_indicator); + Rtcm::set_DF040(ephL1.i_satellite_freq_channel); + Rtcm::set_DF041(gnss_synchroL1); + Rtcm::set_DF042(gnss_synchroL1); + Rtcm::set_DF043(ephL1, obs_time, gnss_synchroL1); + std::bitset<2> DF046_ = std::bitset<2>(0); // code indicator 0: C/A or L2C code 1: P(Y) code direct 2:P(Y) code cross-correlated 3: Correlated P/Y + Rtcm::set_DF047(gnss_synchroL1, gnss_synchroL2); + Rtcm::set_DF048(gnss_synchroL1, gnss_synchroL2); + Rtcm::set_DF049(ephL2, obs_time, gnss_synchroL2); + + std::string content = DF038.to_string() + + DF039.to_string() + + DF040.to_string() + + DF041.to_string() + + DF042.to_string() + + DF043.to_string() + + DF046_.to_string() + + DF047.to_string() + + DF048.to_string() + + DF049.to_string(); + + std::bitset<107> content_msg(content); + return content_msg; +} + + + +// ****************************************************************** +// +// MESSAGE TYPE 1004 (EXTENDED GLONASS L1 & L2 OBSERVATIONS) +// +// ****************************************************************** + +std::string Rtcm::print_MT1012(const Glonass_Gnav_Ephemeris & ephL1, const Glonass_Gnav_Ephemeris & ephL2, double obs_time, const std::map & observables, unsigned short station_id) +{ + unsigned int ref_id = static_cast(station_id); + unsigned int smooth_int = 0; + bool sync_flag = false; + bool divergence_free = false; + + //Get maps with GLONASS L1 and L2 observations + std::map observablesL1; + std::map observablesL2; + std::map::const_iterator observables_iter; + std::map::const_iterator observables_iter2; + + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if((system_.compare("R") == 0) && (sig_.compare("1C") == 0)) + { + observablesL1.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if((system_.compare("R") == 0) && (sig_.compare("2C") == 0)) + { + observablesL2.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + // Get common observables + std::vector< std::pair< Gnss_Synchro, Gnss_Synchro > > common_observables; + std::vector< std::pair< Gnss_Synchro, Gnss_Synchro > >::const_iterator common_observables_iter; + std::map observablesL1_with_L2; + + for(observables_iter = observablesL1.begin(); + observables_iter != observablesL1.end(); + observables_iter++) + { + unsigned int prn_ = observables_iter->second.PRN; + for(observables_iter2 = observablesL2.begin(); + observables_iter2 != observablesL2.end(); + observables_iter2++) + { + if(observables_iter2->second.PRN == prn_) + { + std::pair p; + Gnss_Synchro pr1 = observables_iter->second; + Gnss_Synchro pr2 = observables_iter2->second; + p = std::make_pair(pr1, pr2); + common_observables.push_back(p); + observablesL1_with_L2.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + } + + std::bitset<61> header = Rtcm::get_MT1009_12_header(1012, obs_time, observablesL1_with_L2, ref_id, smooth_int, sync_flag, divergence_free); + std::string data = header.to_string(); + + for(common_observables_iter = common_observables.begin(); + common_observables_iter != common_observables.end(); + common_observables_iter++) + { + std::bitset<130> content = Rtcm::get_MT1012_sat_content(ephL1, ephL2, obs_time, common_observables_iter->first, common_observables_iter->second); + data += content.to_string(); + } + + std::string msg = build_message(data); + if(server_is_running) + { + rtcm_message_queue->push(msg); + } + return msg; +} + + +std::bitset<130> Rtcm::get_MT1012_sat_content(const Glonass_Gnav_Ephemeris & ephL1, const Glonass_Gnav_Ephemeris & ephL2, double obs_time, const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2) +{ + bool code_indicator = false; // code indicator 0: C/A code 1: P(Y) code direct + Rtcm::set_DF038(gnss_synchroL1); + Rtcm::set_DF039(code_indicator); + Rtcm::set_DF040(ephL1.i_satellite_freq_channel); + Rtcm::set_DF041(gnss_synchroL1); + Rtcm::set_DF042(gnss_synchroL1); + Rtcm::set_DF043(ephL1, obs_time, gnss_synchroL1); + Rtcm::set_DF044(gnss_synchroL1); + Rtcm::set_DF045(gnss_synchroL1); + std::bitset<2> DF046_ = std::bitset<2>(0); // code indicator 0: C/A or L2C code 1: P(Y) code direct 2:P(Y) code cross-correlated 3: Correlated P/Y + Rtcm::set_DF047(gnss_synchroL1, gnss_synchroL2); + Rtcm::set_DF048(gnss_synchroL1, gnss_synchroL2); + Rtcm::set_DF049(ephL2, obs_time, gnss_synchroL2); + Rtcm::set_DF050(gnss_synchroL2); + + std::string content = DF038.to_string() + + DF039.to_string() + + DF040.to_string() + + DF041.to_string() + + DF042.to_string() + + DF043.to_string() + + DF044.to_string() + + DF045.to_string() + + DF046_.to_string() + + DF047.to_string() + + DF048.to_string() + + DF049.to_string() + + DF050.to_string(); + + std::bitset<130> content_msg(content); + return content_msg; +} + + // ******************************************************** // // MESSAGE TYPE 1019 (GPS EPHEMERIS) @@ -1299,6 +1703,254 @@ int Rtcm::read_MT1019(const std::string & message, Gps_Ephemeris & gps_eph) } +// ******************************************************** +// +// MESSAGE TYPE 1020 (GLONASS EPHEMERIS) +// +// ******************************************************** + +std::string Rtcm::print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int msg_number = 1020; + + Rtcm::set_DF002(msg_number); + Rtcm::set_DF038(glonass_gnav_eph); + Rtcm::set_DF040(glonass_gnav_eph); + Rtcm::set_DF104(glonass_gnav_eph); + Rtcm::set_DF105(glonass_gnav_eph); + Rtcm::set_DF106(glonass_gnav_eph); + Rtcm::set_DF107(glonass_gnav_eph); + Rtcm::set_DF108(glonass_gnav_eph); + Rtcm::set_DF109(glonass_gnav_eph); + Rtcm::set_DF110(glonass_gnav_eph); + Rtcm::set_DF111(glonass_gnav_eph); + Rtcm::set_DF112(glonass_gnav_eph); + Rtcm::set_DF113(glonass_gnav_eph); + Rtcm::set_DF114(glonass_gnav_eph); + Rtcm::set_DF115(glonass_gnav_eph); + Rtcm::set_DF116(glonass_gnav_eph); + Rtcm::set_DF117(glonass_gnav_eph); + Rtcm::set_DF118(glonass_gnav_eph); + Rtcm::set_DF119(glonass_gnav_eph); + Rtcm::set_DF120(glonass_gnav_eph); + Rtcm::set_DF121(glonass_gnav_eph); + Rtcm::set_DF122(glonass_gnav_eph); + Rtcm::set_DF123(glonass_gnav_eph); + Rtcm::set_DF124(glonass_gnav_eph); + Rtcm::set_DF125(glonass_gnav_eph); + Rtcm::set_DF126(glonass_gnav_eph); + Rtcm::set_DF127(glonass_gnav_eph); + Rtcm::set_DF128(glonass_gnav_eph); + Rtcm::set_DF129(glonass_gnav_eph); + Rtcm::set_DF130(glonass_gnav_eph); + Rtcm::set_DF131(glonass_gnav_eph); + Rtcm::set_DF132(glonass_gnav_eph); + Rtcm::set_DF133(glonass_gnav_eph); + Rtcm::set_DF134(glonass_gnav_eph); + Rtcm::set_DF135(glonass_gnav_eph); + Rtcm::set_DF136(glonass_gnav_eph); + + std::string data; + data.clear(); + data = DF002.to_string() + + DF038.to_string() + + DF040.to_string() + + DF104.to_string() + + DF105.to_string() + + DF106.to_string() + + DF107.to_string() + + DF108.to_string() + + DF109.to_string() + + DF110.to_string() + + DF111.to_string() + + DF112.to_string() + + DF113.to_string() + + DF114.to_string() + + DF115.to_string() + + DF116.to_string() + + DF117.to_string() + + DF118.to_string() + + DF119.to_string() + + DF120.to_string() + + DF121.to_string() + + DF122.to_string() + + DF123.to_string() + + DF124.to_string() + + DF125.to_string() + + DF126.to_string() + + DF127.to_string() + + DF128.to_string() + + DF129.to_string() + + DF130.to_string() + + DF131.to_string() + + DF132.to_string() + + DF133.to_string() + + DF134.to_string() + + DF135.to_string() + + DF136.to_string() + + DF137.to_string(); + + if (data.length() != 360) + { + LOG(WARNING) << "Bad-formatted RTCM MT1020 (360 bits expected, found " << data.length() << ")"; + } + + std::string msg = build_message(data); + if(server_is_running) + { + rtcm_message_queue->push(msg); + } + return msg; +} + + +int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + // Convert message to binary + std::string message_bin = Rtcm::binary_data_to_bin(message); + + if(!Rtcm::check_CRC(message) ) + { + LOG(WARNING) << " Bad CRC detected in RTCM message MT1020"; + return 1; + } + + unsigned int preamble_length = 8; + unsigned int reserved_field_length = 6; + unsigned int index = preamble_length + reserved_field_length; + + unsigned int read_message_length = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 10))); + index += 10; + + if (read_message_length != 61) + { + LOG(WARNING) << " Message MT1020 seems too long (61 bytes expected, " << read_message_length << " received)"; + return 1; + } + + // Check than the message number is correct + unsigned int read_msg_number = Rtcm::bin_to_uint(message_bin.substr(index, 12)); + index += 12; + + if (1020 != read_msg_number) + { + LOG(WARNING) << " This is not a MT1020 message"; + return 1; + } + + // Fill Gps Ephemeris with message data content + glonass_gnav_eph.i_satellite_slot_number = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 6))); + index += 6; + + glonass_gnav_eph.i_satellite_freq_channel = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + index += 5; + + glonass_gnav_eph.i_almanac_health = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.i_alamanac_health_indi = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_P_1 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); + index += 2; + + glonass_gnav_eph.d_t_k = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 1)))*30 + static_cast(Rtcm::bin_to_int(message_bin.substr(index, 5)))*3600 + + static_cast(Rtcm::bin_to_int(message_bin.substr(index, 6)))*60 + ; + index += 12; + + glonass_gnav_eph.d_B_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_P_2 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_t_b = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 7)))*15.0*60.0; + index += 7; + + // TODO Check for type spec for intS24 + glonass_gnav_eph.d_VXn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 24)))*TWO_N20; + index += 24; + + glonass_gnav_eph.d_Xn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 27)))*TWO_N11; + index += 27; + + glonass_gnav_eph.d_AXn = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)))*TWO_N30; + index += 5; + + glonass_gnav_eph.d_VYn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 24)))*TWO_N20; + index += 24; + + glonass_gnav_eph.d_Yn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 27)))*TWO_N11; + index += 27; + + glonass_gnav_eph.d_AYn = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)))*TWO_N30; + index += 5; + + glonass_gnav_eph.d_VZn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 24)))*TWO_N20; + index += 24; + + glonass_gnav_eph.d_Zn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 27)))*TWO_N11; + index += 27; + + glonass_gnav_eph.d_AZn = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)))*TWO_N30; + index += 5; + + glonass_gnav_eph.d_P_3 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_gamma_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11)))*TWO_N30; + index += 11; + + glonass_gnav_eph.d_P = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); + index += 2; + + glonass_gnav_eph.d_l_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_tau_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 22)))* TWO_N30; + index += 22; + + glonass_gnav_eph.d_Delta_tau_n = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 5)))* TWO_N30; + index += 5; + + glonass_gnav_eph.d_E_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + index += 5; + + glonass_gnav_eph.d_P_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_F_T = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 4))); + index += 4; + + glonass_gnav_eph.d_N_T = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); + index += 11; + + glonass_gnav_eph.d_M = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); + index += 2; + + glonass_gnav_eph.d_i_0 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass_gnav_eph.d_N_A = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); + index += 11; + + glonass_gnav_eph.d_tau_c = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 32)))* TWO_N31; + index += 32; + + glonass_gnav_eph.d_N_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + index += 5; + + glonass_gnav_eph.d_tau_gps = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 22)))* TWO_N30; + index += 22; + + glonass_gnav_eph.d_l_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + + return 0; +} + + // ******************************************************** // // MESSAGE TYPE 1029 (UNICODE TEXT STRING) @@ -1582,6 +2234,7 @@ int Rtcm::read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph) std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -1747,6 +2400,7 @@ std::string Rtcm::get_MSM_1_content_signal_data(const std::map & observables, unsigned int ref_id, @@ -1782,7 +2436,7 @@ std::string Rtcm::print_MSM_2( const Gps_Ephemeris & gps_eph, std::string sat_data = Rtcm::get_MSM_1_content_sat_data(observables); - std::string signal_data = Rtcm::get_MSM_2_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables); + std::string signal_data = Rtcm::get_MSM_2_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables); std::string message = build_message(header + sat_data + signal_data); if(server_is_running) @@ -1794,7 +2448,12 @@ std::string Rtcm::print_MSM_2( const Gps_Ephemeris & gps_eph, } -std::string Rtcm::get_MSM_2_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables) +std::string Rtcm::get_MSM_2_content_signal_data(const Gps_Ephemeris & ephNAV, + const Gps_CNAV_Ephemeris & ephCNAV, + const Galileo_Ephemeris & ephFNAV, + const Glonass_Gnav_Ephemeris & ephGNAV, + double obs_time, + const std::map & observables) { std::string signal_data; std::string first_data_type; @@ -1842,6 +2501,7 @@ std::string Rtcm::get_MSM_2_content_signal_data(const Gps_Ephemeris & ephNAV, co std::string Rtcm::print_MSM_3( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -1877,7 +2537,7 @@ std::string Rtcm::print_MSM_3( const Gps_Ephemeris & gps_eph, std::string sat_data = Rtcm::get_MSM_1_content_sat_data(observables); - std::string signal_data = Rtcm::get_MSM_3_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables); + std::string signal_data = Rtcm::get_MSM_3_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables); std::string message = build_message(header + sat_data + signal_data); if(server_is_running) @@ -1889,7 +2549,12 @@ std::string Rtcm::print_MSM_3( const Gps_Ephemeris & gps_eph, } -std::string Rtcm::get_MSM_3_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables) +std::string Rtcm::get_MSM_3_content_signal_data(const Gps_Ephemeris & ephNAV, + const Gps_CNAV_Ephemeris & ephCNAV, + const Galileo_Ephemeris & ephFNAV, + const Glonass_Gnav_Ephemeris & ephGNAV, + double obs_time, + const std::map & observables) { std::string signal_data; std::string first_data_type; @@ -1939,6 +2604,7 @@ std::string Rtcm::get_MSM_3_content_signal_data(const Gps_Ephemeris & ephNAV, co std::string Rtcm::print_MSM_4( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -1974,7 +2640,7 @@ std::string Rtcm::print_MSM_4( const Gps_Ephemeris & gps_eph, std::string sat_data = Rtcm::get_MSM_4_content_sat_data(observables); - std::string signal_data = Rtcm::get_MSM_4_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables); + std::string signal_data = Rtcm::get_MSM_4_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables); std::string message = build_message(header + sat_data + signal_data); if(server_is_running) @@ -2026,7 +2692,12 @@ std::string Rtcm::get_MSM_4_content_sat_data(const std::map & } -std::string Rtcm::get_MSM_4_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables) +std::string Rtcm::get_MSM_4_content_signal_data(const Gps_Ephemeris & ephNAV, + const Gps_CNAV_Ephemeris & ephCNAV, + const Galileo_Ephemeris & ephFNAV, + const Glonass_Gnav_Ephemeris & ephGNAV, + double obs_time, + const std::map & observables) { std::string signal_data; std::string first_data_type; @@ -2079,6 +2750,7 @@ std::string Rtcm::get_MSM_4_content_signal_data(const Gps_Ephemeris & ephNAV, co std::string Rtcm::print_MSM_5( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -2114,7 +2786,7 @@ std::string Rtcm::print_MSM_5( const Gps_Ephemeris & gps_eph, std::string sat_data = Rtcm::get_MSM_5_content_sat_data(observables); - std::string signal_data = Rtcm::get_MSM_5_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables); + std::string signal_data = Rtcm::get_MSM_5_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables); std::string message = build_message(header + sat_data + signal_data); if(server_is_running) @@ -2172,7 +2844,12 @@ std::string Rtcm::get_MSM_5_content_sat_data(const std::map & } -std::string Rtcm::get_MSM_5_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables) +std::string Rtcm::get_MSM_5_content_signal_data(const Gps_Ephemeris & ephNAV, + const Gps_CNAV_Ephemeris & ephCNAV, + const Galileo_Ephemeris & ephFNAV, + const Glonass_Gnav_Ephemeris & ephGNAV, + double obs_time, + const std::map & observables) { std::string signal_data; std::string first_data_type; @@ -2229,6 +2906,7 @@ std::string Rtcm::get_MSM_5_content_signal_data(const Gps_Ephemeris & ephNAV, co std::string Rtcm::print_MSM_6( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -2264,7 +2942,7 @@ std::string Rtcm::print_MSM_6( const Gps_Ephemeris & gps_eph, std::string sat_data = Rtcm::get_MSM_4_content_sat_data(observables); - std::string signal_data = Rtcm::get_MSM_6_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables); + std::string signal_data = Rtcm::get_MSM_6_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables); std::string message = build_message(header + sat_data + signal_data); if(server_is_running) @@ -2276,7 +2954,12 @@ std::string Rtcm::print_MSM_6( const Gps_Ephemeris & gps_eph, } -std::string Rtcm::get_MSM_6_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables) +std::string Rtcm::get_MSM_6_content_signal_data(const Gps_Ephemeris & ephNAV, + const Gps_CNAV_Ephemeris & ephCNAV, + const Galileo_Ephemeris & ephFNAV, + const Glonass_Gnav_Ephemeris & ephGNAV, + double obs_time, + const std::map & observables) { std::string signal_data; std::string first_data_type; @@ -2330,6 +3013,7 @@ std::string Rtcm::get_MSM_6_content_signal_data(const Gps_Ephemeris & ephNAV, co std::string Rtcm::print_MSM_7( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -2365,7 +3049,7 @@ std::string Rtcm::print_MSM_7( const Gps_Ephemeris & gps_eph, std::string sat_data = Rtcm::get_MSM_5_content_sat_data(observables); - std::string signal_data = Rtcm::get_MSM_7_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, obs_time, observables); + std::string signal_data = Rtcm::get_MSM_7_content_signal_data(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables); std::string message = build_message(header + sat_data + signal_data); if(server_is_running) @@ -2377,7 +3061,12 @@ std::string Rtcm::print_MSM_7( const Gps_Ephemeris & gps_eph, } -std::string Rtcm::get_MSM_7_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables) +std::string Rtcm::get_MSM_7_content_signal_data(const Gps_Ephemeris & ephNAV, + const Gps_CNAV_Ephemeris & ephCNAV, + const Galileo_Ephemeris & ephFNAV, + const Glonass_Gnav_Ephemeris & ephGNAV, + double obs_time, + const std::map & observables) { std::string signal_data; std::string first_data_type; @@ -3028,6 +3717,215 @@ int Rtcm::set_DF031(unsigned int antenna_setup_id) } +int Rtcm::set_DF034(double obs_time) +{ + // TOW in milliseconds from the beginning of the GLONASS day, measured in GLONASS time + unsigned long int tk = static_cast(std::round(obs_time * 1000)); + if(tk > 86400999) + { + LOG(WARNING) << "To large GLONASS Epoch Time (tk)! Set to the last millisecond of the day"; + tow = 86400999; + } + DF034 = std::bitset<27>(tk); + return 0; +} + + +int Rtcm::set_DF035(const std::map & observables) +{ + //Number of satellites observed in current epoch + unsigned short int nsats = 0; + std::map::const_iterator observables_iter; + for(observables_iter = observables.begin(); + observables_iter != observables.end(); + observables_iter++) + { + nsats++; + } + if (nsats > 31) + { + LOG(WARNING) << "The number of processed GLONASS satellites must be between 0 and 31, but it seems that you are processing " << nsats; + nsats = 31; + } + DF035 = std::bitset<5>(nsats); + return 0; +} + + +int Rtcm::set_DF036(bool divergence_free_smoothing_indicator) +{ + // 0 - Divergence-free smoothing not used 1 - Divergence-free smoothing used + DF036 = std::bitset<1>(divergence_free_smoothing_indicator); + return 0; +} + + +int Rtcm::set_DF037(short int smoothing_interval) +{ + DF037 = std::bitset<3>(smoothing_interval); + return 0; +} + + +int Rtcm::set_DF038(const Gnss_Synchro & gnss_synchro) +{ + unsigned int prn_ = gnss_synchro.PRN; + if(prn_ > 24) + { + LOG(WARNING) << "GLONASS satellite ID (Slot Number) must be between 1 and 24, but PRN " << prn_ << " was found"; + } + DF038 = std::bitset<6>(prn_); + return 0; +} + + +int Rtcm::set_DF038(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int prn_ = glonass_gnav_eph.i_satellite_slot_number; + if(prn_ > 24) + { + LOG(WARNING) << "GLONASS satellite ID (Slot Number) must be between 0 and 24, but PRN " << prn_ << " was found"; + } + DF038 = std::bitset<6>(prn_); + return 0; +} + + +int Rtcm::set_DF039(bool code_indicator) +{ + DF039 = std::bitset<1>(code_indicator); + return 0; +} + + +int Rtcm::set_DF040(unsigned short frequency_channel_number) +{ + unsigned int freq_ = glonass_gnav_eph.i_satellite_freq_channel; + if(prn_ > 20) + { + LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; + } + + DF040 = std::bitset<5>(freq_); + return 0; +} + + +int Rtcm::set_DF041(const Gnss_Synchro & gnss_synchro) +{ + double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 599584.92 ); + unsigned long int glonass_L1_pseudorange = static_cast(std::round(( gnss_synchro.Pseudorange_m - ambiguity * 599584.92) / 0.02 )); + DF041 = std::bitset<25>(glonass_L1_pseudorange); + return 0; +} + +//TODO Need to fix the lambda value since it needs to include frequency channel +int Rtcm::set_DF042(const Gnss_Synchro & gnss_synchro) +{ + const double lambda = GLONASS_C_m_s / GLONASS_L1_FREQ_HZ; + double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 599584.92 ); + double glonass_L1_pseudorange = std::round(( gnss_synchro.Pseudorange_m - ambiguity * 599584.92) / 0.02 ); + double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 299792.458; + double L1_phaserange_c = gnss_synchro.Carrier_phase_rads / GLONASS_TWO_PI; + double L1_phaserange_c_r = std::fmod(L1_phaserange_c - glonass_L1_pseudorange_c / lambda + 1500.0, 3000.0) - 1500.0; + long int glonass_L1_phaserange_minus_L1_pseudorange = static_cast(std::round(L1_phaserange_c_r * lambda / 0.0005 )); + DF042 = std::bitset<20>(glonass_L1_phaserange_minus_L1_pseudorange); + return 0; +} + + +int Rtcm::set_DF043(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro) +{ + unsigned int lock_time_indicator; + unsigned int lock_time_period_s = Rtcm::lock_time(eph, obs_time, gnss_synchro); + lock_time_indicator = Rtcm::lock_time_indicator(lock_time_period_s); + DF043 = std::bitset<7>(lock_time_indicator); + return 0; +} + + +int Rtcm::set_DF044(const Gnss_Synchro & gnss_synchro) +{ + unsigned int glonass_L1_pseudorange_ambiguity = static_cast(std::floor(gnss_synchro.Pseudorange_m / 599584.916)); + DF044 = std::bitset<7>(glonass_L1_pseudorange_ambiguity); + return 0; +} + + +int Rtcm::set_DF045(const Gnss_Synchro & gnss_synchro) +{ + double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz; + if (CN0_dB_Hz_est > 63.75) + { + LOG(WARNING) << "GLONASS L1 CNR must be between 0 and 63.75, but CNR " << CN0_dB_Hz_est << " was found. Setting to 63.75 dB-Hz"; + CN0_dB_Hz_est = 63.75; + } + unsigned int CN0_dB_Hz = static_cast(std::round(CN0_dB_Hz_est / 0.25 )); + DF045 = std::bitset<8>(CN0_dB_Hz); + return 0; +} + + +int Rtcm::set_DF047(const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2) +{ + double ambiguity = std::floor( gnss_synchroL1.Pseudorange_m / 599584.92 ); + double glonass_L1_pseudorange = std::round(( gnss_synchroL1.Pseudorange_m - ambiguity * 599584.92) / 0.02 ); + double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 599584.92; + + double l2_l1_pseudorange = gnss_synchroL2.Pseudorange_m - glonass_L1_pseudorange_c; + int pseudorange_difference = 0xFFFFE000; // invalid value; + if(std::fabs(l2_l1_pseudorange) <= 163.82) + { + pseudorange_difference = static_cast(std::round(l2_l1_pseudorange / 0.02)); + } + DF047 = std::bitset<14>(pseudorange_difference); + return 0; +} + +//TODO Need to consider frequency channel in this fields +int Rtcm::set_DF048(const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2) +{ + const double lambda2 = GLONASS_C_m_s / GLONASS_L2_FREQ_HZ; + int l2_phaserange_minus_l1_pseudorange = 0xFFF80000; + double ambiguity = std::floor( gnss_synchroL1.Pseudorange_m / 599584.92 ); + double glonass_L1_pseudorange = std::round(( gnss_synchroL1.Pseudorange_m - ambiguity * 599584.92) / 0.02 ); + double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 599584.92; + double L2_phaserange_c = gnss_synchroL2.Carrier_phase_rads / GLONASS_TWO_PI; + double L1_phaserange_c_r = std::fmod(L2_phaserange_c - glonass_L1_pseudorange_c / lambda2 + 1500.0, 3000.0) - 1500.0; + + if (std::fabs(L1_phaserange_c_r * lambda2) <= 262.1435 ) + { + l2_phaserange_minus_l1_pseudorange = static_cast(std::round(L1_phaserange_c_r * lambda2 / 0.0005)); + } + + DF048 = std::bitset<20>(l2_phaserange_minus_l1_pseudorange); + return 0; +} + + +int Rtcm::set_DF049(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro) +{ + unsigned int lock_time_indicator; + unsigned int lock_time_period_s = Rtcm::lock_time(eph, obs_time, gnss_synchro); + lock_time_indicator = Rtcm::lock_time_indicator(lock_time_period_s); + DF049 = std::bitset<7>(lock_time_indicator); + return 0; +} + + +int Rtcm::set_DF050(const Gnss_Synchro & gnss_synchro) +{ + double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz; + if (CN0_dB_Hz_est > 63.75) + { + CN0_dB_Hz_est = 63.75; + } + unsigned int CN0_dB_Hz = static_cast(std::round(CN0_dB_Hz_est / 0.25 )); + DF050 = std::bitset<8>(CN0_dB_Hz); + return 0; +} + + int Rtcm::set_DF051(const Gps_Ephemeris & gps_eph, double obs_time) { const double gps_t = obs_time; diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index 8b3333ffe..d3b84beb1 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -49,6 +49,7 @@ #include "galileo_fnav_message.h" #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" +#include "glonass_gnav_navigation_message.h" /*! @@ -56,7 +57,7 @@ * defined in the RTCM 3.2 Standard, plus some utilities to handle messages. * * Generation of the following Message Types: - * 1001, 1002, 1003, 1004, 1005, 1006, 1008, 1019, 1029, 1045 + * 1001, 1002, 1003, 1004, 1005, 1006, 1008, 1019, 1020, 1029, 1045 * * Decoding of the following Message Types: * 1019, 1045 @@ -128,6 +129,47 @@ public: */ std::string print_MT1008(unsigned int ref_id, const std::string & antenna_descriptor, unsigned int antenna_setup_id, const std::string & antenna_serial_number); + /*! + * \brief Prints L1-Only GLONASS RTK Observables + * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred. + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return string with message contents + */ + std::string print_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id); + /*! + * \brief Prints Extended L1-Only GLONASS RTK Observables + * \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases. + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return string with message contents + */ + std::string print_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id); + /*! + * \brief Prints L1&L2 GLONASS RTK Observables + * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return string with message contents + */ + std::string print_MT1011(Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); + /*! + * \brief Prints Extended L1&L2 GLONASS RTK Observables + * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. + * \note Code added as part of GSoC 2017 program + * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \params obs_time Time of observation at the moment of printing + * \params observables Set of observables as defined by the platform + * \return string with message contents + */ + std::string print_MT1012(Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); + /*! * \brief Prints message type 1019 (GPS Ephemeris), should be broadcast in the event that * the IODC does not match the IODE, and every 2 minutes. @@ -139,6 +181,23 @@ public: */ int read_MT1019(const std::string & message, Gps_Ephemeris & gps_eph); + /*! + * \brief Prints message type 1020 (GLONASS Ephemeris). + * \note Code added as part of GSoC 2017 program + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \return Returns message type as a string type + */ + std::string print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + /*! + * \brief Verifies and reads messages of type 1020 (GLONASS Ephemeris). + * \note Code added as part of GSoC 2017 program + * \param message Message to read as a string type + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \return Returns 1 if anything goes wrong, 0 otherwise. + */ + int read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glonass_gnav_eph); + /*! * \brief Prints message type 1029 (Unicode Text String) */ @@ -160,6 +219,7 @@ public: std::string print_MSM_1( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -175,6 +235,7 @@ public: std::string print_MSM_2( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -190,6 +251,7 @@ public: std::string print_MSM_3( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -205,6 +267,7 @@ public: std::string print_MSM_4( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -220,6 +283,7 @@ public: std::string print_MSM_5( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -235,6 +299,7 @@ public: std::string print_MSM_6( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -250,6 +315,7 @@ public: std::string print_MSM_7( const Gps_Ephemeris & gps_eph, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Galileo_Ephemeris & gal_eph, + const Glonass_Gnav_Ephemeris & glo_gnav_eph, double obs_time, const std::map & observables, unsigned int ref_id, @@ -262,6 +328,15 @@ public: unsigned int lock_time(const Gps_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); // get_MT1005_test(); + /*! + * \brief Generates contents of message header for types 1009, 1010, 1011 and 1012. GLONASS RTK Message + * \note Code added as part of GSoC 2017 program + * \param msg_number Message type number, acceptable options include 1009 to 1012 + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform + * \param ref_id + * \param smooth_int + * \param divergence_free + * \return Returns the message header content as set of bits + */ + std::bitset<61> get_MT1009_12_header(unsigned int msg_number, + double obs_time, + const std::map & observables, + unsigned int ref_id, + unsigned int smooth_int, + bool sync_flag, + bool divergence_free); + + /*! + * \brief Get the contents of the satellite specific portion of a type 1009 Message (GLONASS Basic RTK, L1 Only) + * \details Contents generated for each satellite. See table 3.5-11 + * \note Code added as part of GSoC 2017 program + * \param ephGNAV Ephemeris for GLONASS GNAV in L1 satellites + * \param obs_time Time of observation at the moment of printing + * \param gnss_synchro Information generated by channels while processing the satellite + * \return Returns the message content as set of bits + */ + std::bitset<64> get_MT1009_sat_content(const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const Gnss_Synchro & gnss_synchro); + /*! + * \brief Get the contents of the satellite specific portion of a type 1010 Message (GLONASS Extended RTK, L1 Only) + * \details Contents generated for each satellite. See table 3.5-12 + * \note Code added as part of GSoC 2017 program + * \param ephGNAV Ephemeris for GLONASS GNAV in L1 satellites + * \param obs_time Time of observation at the moment of printing + * \param gnss_synchro Information generated by channels while processing the satellite + * \return Returns the message content as set of bits + */ + std::bitset<79> get_MT1010_sat_content(const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const Gnss_Synchro & gnss_synchro); + /*! + * \brief Get the contents of the satellite specific portion of a type 1011 Message (GLONASS Basic RTK, L1 & L2) + * \details Contents generated for each satellite. See table 3.5-13 + * \note Code added as part of GSoC 2017 program + * \param ephGNAVL1 Ephemeris for GLONASS GNAV in L1 satellites + * \param ephGNAVL2 Ephemeris for GLONASS GNAV in L2 satellites + * \param obs_time Time of observation at the moment of printing + * \param gnss_synchroL1 Information generated by channels while processing the GLONASS GNAV L1 satellite + * \param gnss_synchroL2 Information generated by channels while processing the GLONASS GNAV L2 satellite + * \return Returns the message content as set of bits + */ + std::bitset<107> get_MT1011_sat_content(const Glonass_Gnav_Ephemeris & ephGNAVL1, const Glonass_Gnav_Ephemeris & ephGNAVL2, double obs_time, const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2); + /*! + * \brief Get the contents of the satellite specific portion of a type 1012 Message (GLONASS Extended RTK, L1 & L2) + * \details Contents generated for each satellite. See table 3.5-14 + * \note Code added as part of GSoC 2017 program + * \param ephGNAVL1 Ephemeris for GLONASS GNAV in L1 satellites + * \param ephGNAVL2 Ephemeris for GLONASS GNAV in L2 satellites + * \param obs_time Time of observation at the moment of printing + * \param gnss_synchroL1 Information generated by channels while processing the GLONASS GNAV L1 satellite + * \param gnss_synchroL2 Information generated by channels while processing the GLONASS GNAV L2 satellite + * \return Returns the message content as set of bits + */ + std::bitset<130> get_MT1012_sat_content(const Glonass_Gnav_Ephemeris & ephGNAVL1, const Glonass_Gnav_Ephemeris & ephGNAVL2, double obs_time, const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2); + std::string get_MSM_header(unsigned int msg_number, double obs_time, const std::map & observables, @@ -318,12 +458,12 @@ private: std::string get_MSM_5_content_sat_data(const std::map & observables); std::string get_MSM_1_content_signal_data(const std::map & observables); - std::string get_MSM_2_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables); - std::string get_MSM_3_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables); - std::string get_MSM_4_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables); - std::string get_MSM_5_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables); - std::string get_MSM_6_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables); - std::string get_MSM_7_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const std::map & observables); + std::string get_MSM_2_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const std::map & observables); + std::string get_MSM_3_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const std::map & observables); + std::string get_MSM_4_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const std::map & observables); + std::string get_MSM_5_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const std::map & observables); + std::string get_MSM_6_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const std::map & observables); + std::string get_MSM_7_content_signal_data(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const std::map & observables); // // Utilities @@ -335,10 +475,13 @@ private: boost::posix_time::ptime compute_GPS_time(const Gps_Ephemeris& eph, double obs_time) const; boost::posix_time::ptime compute_GPS_time(const Gps_CNAV_Ephemeris & eph, double obs_time) const; boost::posix_time::ptime compute_Galileo_time(const Galileo_Ephemeris& eph, double obs_time) const; + boost::posix_time::ptime compute_GLONASS_time(const Glonass_Gnav_Ephemeris& eph, double obs_time) const; boost::posix_time::ptime gps_L1_last_lock_time[64]; boost::posix_time::ptime gps_L2_last_lock_time[64]; boost::posix_time::ptime gal_E1_last_lock_time[64]; boost::posix_time::ptime gal_E5_last_lock_time[64]; + boost::posix_time::ptime glo_L1_last_lock_time[64]; + boost::posix_time::ptime glo_L2_last_lock_time[64]; unsigned int lock_time_indicator(unsigned int lock_time_period_s); unsigned int msm_lock_time_indicator(unsigned int lock_time_period_s); unsigned int msm_extended_lock_time_indicator(unsigned int lock_time_period_s); @@ -870,6 +1013,59 @@ private: std::bitset<8> DF032; + std::bitset<27> DF034; //!< GLONASS Epoch Time (tk) + int set_DF034(double obs_time); + + std::bitset<5> DF035; //!< No. of GLONASS Satellite Signals Processed + int set_DF035(const std::map & observables); + + std::bitset<1> DF036; //!< GLONASS Divergence-free Smoothing Indicator + int set_DF036(bool divergence_free_smoothing_indicator); + + std::bitset<3> DF037; //!< GLONASS Smoothing Interval + int set_DF037(unsigned short smoothing_interval); + + std::bitset<6> DF038; //!< GLONASS Satellite ID (Satellite Slot Number) + int set_DF038(const Gnss_Synchro & gnss_synchro); + int set_DF038(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF039; //!< GLONASS L1 Code Indicator + int set_DF039(bool code_indicator); + + std::bitset<5> DF040; //!< GLONASS Satellite Frequency Number + int set_DF040(unsigned int frequency_channel_number); + int set_DF040(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<25> DF041; //!< GLONASS L1 Pseudorange + int set_DF041(const Gnss_Synchro & gnss_synchro); + + std::bitset<20> DF042; //!< GLONASS L1 PhaseRange - L1 Pseudorange + int set_DF042(const Gnss_Synchro & gnss_synchro); + + std::bitset<7> DF043; //!< GLONASS L1 Lock Time Indicator + int set_DF043(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); + + std::bitset<7> DF044; //!< GLONASS Integer L1 Pseudorange Modulus Ambiguity + int set_DF044(const Gnss_Synchro & gnss_synchro); + + std::bitset<8> DF045; //!< GLONASS L1 CNR + int set_DF045(const Gnss_Synchro & gnss_synchro); + + std::bitset<2> DF046; //!< GLONASS L2 code indicator + int set_DF046(unsigned short code_indicator); + + std::bitset<14> DF047; //!< GLONASS L2 - L1 Pseudorange Difference + int set_DF047(const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2); + + std::bitset<20> DF048; //!< GLONASS L2 PhaseRange - L1 Pseudorange + int set_DF048(const Gnss_Synchro & gnss_synchroL1, const Gnss_Synchro & gnss_synchroL2); + + std::bitset<7> DF049; //!< GLONASS L2 Lock Time Indicator + int set_DF049(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); + + std::bitset<8> DF050; //!< GLONASS L2 CNR + int set_DF050(const Gnss_Synchro & gnss_synchro); + std::bitset<16> DF051; int set_DF051(const Gps_Ephemeris & gps_eph, double obs_time); @@ -964,6 +1160,105 @@ private: std::bitset<1> DF103; int set_DF103(const Gps_Ephemeris & gps_eph); + std::bitset<1> DF104; //!< GLONASS Almanac Health + int set_DF104(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF105; //!< GLONASS Almanac Health Availability Indicator + int set_DF105(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<2> DF106; //!< GLONASS P1 Word + int set_DF106(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<12> DF107; //!< GLONASS Epoch (tk) + int set_DF107(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF108; //!< GLONASS MSB of Bn Word + int set_DF108(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF109; //!< GLONASS P2 Word + int set_DF109(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<7> DF110; //!< GLONASS Ephmeris Epoch (tb) + int set_DF110(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<24> DF111; //!< GLONASS Xn first derivative + int set_DF111(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<27> DF112; //!< GLONASS Xn + int set_DF112(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<5> DF113; //!< GLONASS Xn second derivative + int set_DF113(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<24> DF114; //!< GLONASS Yn first derivative + int set_DF114(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<27> DF115; //!< GLONASS Yn + int set_DF115(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<5> DF116; //!< GLONASS Yn second derivative + int set_DF116(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<24> DF117; //!< GLONASS Zn first derivative + int set_DF117(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<27> DF118; //!< GLONASS Zn + int set_DF118(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<5> DF119; //!< GLONASS Zn second derivative + int set_DF119(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF120; //!< GLONASS P3 + int set_DF120(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<11> DF121; //!< GLONASS GAMMA_N + int set_DF121(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<2> DF122; //!< GLONASS P + int set_DF122(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF123; //!< GLONASS ln (third string) + int set_DF123(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<22> DF124; //!< GLONASS TAU_N + int set_DF124(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<5> DF125; //!< GLONASS DELTA_TAU_N + int set_DF125(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<5> DF126; //!< GLONASS Eccentricity + int set_DF126(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF127; //!< GLONASS P4 + int set_DF127(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<4> DF128; //!< GLONASS F_T + int set_DF128(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF129; //!< GLONASS N_T + int set_DF129(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<2> DF130; //!< GLONASS M + int set_DF130(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF131; //!< GLONASS Availability of additional data + int set_DF131(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<11> DF132; //!< GLONASS N_A + int set_DF132(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<32> DF133; //!< GLONASS TAU_C + int set_DF133(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<5> DF134; //!< GLONASS N_4 + int set_DF134(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<22> DF135; //!< GLONASS TAU_GPS + int set_DF135(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + + std::bitset<1> DF136; //!< GLONASS L_N (FIFTH STRING) + int set_DF136(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + std::bitset<1> DF137; int set_DF137(const Gps_Ephemeris & gps_eph); diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc new file mode 100644 index 000000000..b67b2a696 --- /dev/null +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc @@ -0,0 +1,138 @@ +/*! + * \file glonass_gnav_navigation_message_test.cc + * \brief This file implements tests for the decoding of the GLONASS GNAV navigation message + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include +#include +#include "gnss_signal_processing.h" +#include "glonass_gnav_navigation_message.h" + +/*! + * \brief Testing CRC computation for GLONASS GNAV data bits of a string + * \test The provided string was generated with a version of MATLAB GNSS-SDR that + * the author coded to perform proper decoding of GLONASS GNAV signals. + */ +TEST(GlonassGnavNavigationMessageTest, CRCTest) +{ + // Variables declarations in code + bool test_result; + std::string str5("0010100100001100000000000000000000000000110011110001100000000000000001100100011000000"); + Glonass_Gnav_Navigation_Message gnav_nav_message; + gnav_nav_message.reset(); + + // Call function to test + test_result = gnav_nav_message.CRC_test(std::bitset (str5)); + + // Check results in unit test assetions + ASSERT_TRUE(test_result); +} + +/*! + * \brief Testing string decoding for GLONASS GNAV messages + * \test The provided string (str1.....str15) was generated with a version of + * MATLAB GNSS-SDR that the author coded to perform proper decoding of GLONASS + * GNAV signals. The same assumption is to be applied for ephemeris and almanac + * data provided. + */ +TEST(GlonassGnavNavigationMessageTest, String1Decoder) +{ + // Variable declarations + std::string str1("0000100000001000011001000011111011010101110100000010101011000100011010101011000101111"); + Glonass_Gnav_Navigation_Message gnav_nav_message; + Glonass_Gnav_Ephemeris gnav_ephemeris; + + // Fill out ephemeris values for truth + gnav_ephemeris.d_P_1 = static_cast(read_navigation_unsigned(string_bits, P1)); + gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K_HR)) * 3600 + + gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)) * 2e-20; + gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)) * 2e-30; + gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * 2e-11; + + // Call target test method + gnav_nav_message.string_decoder(str1.c_str()); + + // Perform assertions of decoded fields + ASSERT_TRUE(gnav_ephemeris.d_t_k - gnav_nav_message.gnav_ephemeris.d_t_k < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_VXn - gnav_nav_message.gnav_ephemeris.d_VXn < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_AXn - gnav_nav_message.gnav_ephemeris.d_AXn < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_Xn - gnav_nav_message.gnav_ephemeris.d_Xn < DBL_EPSILON ); +} + +/*! + * \brief Testing string decoding for GLONASS GNAV messages + * \test The provided string (str1.....str15) was generated with a version of + * MATLAB GNSS-SDR that the author coded to perform proper decoding of GLONASS + * GNAV signals. The same assumption is to be applied for ephemeris and almanac + * data provided. + */ +TEST(GlonassGnavNavigationMessageTest, String2Decoder) +{ + // Variable declarations + std::string str2("0001000010001001000001010101100001011001011000000010101100110000001011110000110011110"); + Glonass_Gnav_Navigation_Message gnav_nav_message; + Glonass_Gnav_Ephemeris gnav_ephemeris; + + // Fill out ephemeris values for truth + gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); + gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); + gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*15*60; + gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* 2e-20; + gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * 2e-30; + gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, X_N)) * 2e-11; + + // Call target test method + gnav_nav_message.string_decoder(str2.c_str()) + + // Perform assertions of decoded fields + ASSERT_TRUE(gnav_ephemeris.d_B_n - gnav_nav_message.gnav_ephemeris.d_B_n < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_P_2 - gnav_nav_message.gnav_ephemeris.d_P_2 < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_t_b - gnav_nav_message.gnav_ephemeris.d_t_b < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_VYn - gnav_nav_message.gnav_ephemeris.d_VYn < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_AYn - gnav_nav_message.gnav_ephemeris.d_AYn < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_Yn - gnav_nav_message.gnav_ephemeris.d_Yn < DBL_EPSILON ); +} + +std::string str2("0001000010001001000001010101100001011001011000000010101100110000001011110000110011110"); +std::string str3("0001110000000001001101001110100011111011010011001101001101110110010011110011100100011"); +std::string str4("0010010000101011100100000100000100000000000000000000011000100100001100101010100011101"); +std::string str5("0010100100001100000000000000000000000000110011110001100000000000000001100100011000000"); +std::string str6("0011010100110100001100111100011100001101011000000110101111001000000101100011111011001"); +std::string str7("0011101101010001000010000110101111110000101101001011111110101110100010111100010001101"); +std::string str8("0100010100111000000001111110001101000000110000001000100111011100001010101111010011010"); +std::string str9("0100111010001001011100010000010100010101111101001011111110101011100010100101000110101"); +std::string str10("0101010101000000000011101111111101111001011000001000101010001100001111000110101111110"); +std::string str11("0101110111011011011100011001111011101111001101001011111111000110100100000110010001111"); +std::string str12("0110010101001100000011110110100110100100010100001000111110000100001110001010111000001"); +std::string str13("0110111011100100111110100001000110100010011101001011111110100100101010011010001101001"); +std::string str14("0111010101010000000100011000011110100110111100001110110100001000001111001101010000101"); +std::string str15("0111101110101010001110101010100111101100001101001011111111100010101010011001010011101"); From e58cd5450d754d714b65c2c71d99cf07896c38d8 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 8 Aug 2017 13:02:59 -0700 Subject: [PATCH 30/48] Adding RTCM Data Fields for GLONASS processing Adds GLONASS related data fields (DFxxx) for RTCM messages including MS1020, MS1087 and general MSM message types --- .../libs/rtklib/rtklib_conversions.cc | 2 +- src/core/system_parameters/GLONASS_L1_CA.h | 2 + .../glonass_gnav_ephemeris.cc | 3 +- .../glonass_gnav_ephemeris.h | 6 +- .../glonass_gnav_navigation_message.cc | 4 +- src/core/system_parameters/rtcm.cc | 491 +++++++++++++++--- src/core/system_parameters/rtcm.h | 32 +- 7 files changed, 453 insertions(+), 87 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index f7da4d8df..f4ffafae8 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -74,7 +74,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number; /* satellite number */ rtklib_sat.iode = glonass_gnav_eph.d_t_b; /* IODE (0-6 bit of tb field) */ rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */ - rtklib_sat.svh = glonass_gnav_eph.d_l_n; /* satellite health*/ + rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ rtklib_sat.sva = glonass_gnav_eph.d_F_T; /* satellite accuracy*/ rtklib_sat.age = glonass_gnav_eph.d_E_n; /* satellite age*/ rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn*1000; /* satellite position (ecef) (m) */ diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index be05cc58a..7532ceb3a 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -78,6 +78,8 @@ const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-ma const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit // carrier and code frequencies +const double GLONASS_L2_FREQ_HZ = FREQ2_GLO; //!< L1 [Hz] + const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] const double GLONASS_L1_DFREQ_HZ = DFRQ1_GLO; //!< Freq Bias for GLONASS L1 [Hz] const double GLONASS_L1_CA_CODE_RATE_HZ = 0.511e6; //!< GLONASS L1 C/A code rate [chips/s] diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index 1127751fe..ba376b620 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -71,7 +71,8 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - d_l_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + d_l3rd_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + d_l5th_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] // clock terms derived from ephemeris data d_satClkDrift = 0.0; //!< GLONASS clock error diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 8735d81cb..153f5b5b1 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -95,7 +95,8 @@ public: double d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] double d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + double d_l3rd_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + double 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] // Inmediate deliverables of ephemeris information //TODO check how freq channel is managed in gnav message. I think it is a number greater thn 0 @@ -164,7 +165,8 @@ public: archive & make_nvp("d_P_2", d_P_2); //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] archive & make_nvp("d_P_3", d_P_3); //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] archive & make_nvp("d_P_4", d_P_4); //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - archive & make_nvp("d_l_n", d_l_n); //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + archive & make_nvp("d_l3rd_n", d_l3rd_n); //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + 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] } /*! diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 1409eaa1d..7cdbec768 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -394,7 +394,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * TWO_N30; gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); + gnav_ephemeris.d_l3rd_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)) * TWO_N20; gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)) * TWO_N30; gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)) * TWO_N11; @@ -428,7 +428,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_utc_model.d_tau_c = static_cast(read_navigation_signed(string_bits, TAU_C)) * TWO_N31; gnav_utc_model.d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); gnav_utc_model.d_tau_gps = static_cast(read_navigation_signed(string_bits, TAU_GPS)) * TWO_N30; - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l5th_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); // Compute Year and DoY based on Algorithm A3.11 of GLONASS ICD if(flag_ephemeris_str_4 == true) diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index a621e4e5c..446c8f667 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -1709,15 +1709,18 @@ int Rtcm::read_MT1019(const std::string & message, Gps_Ephemeris & gps_eph) // // ******************************************************** -std::string Rtcm::print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +std::string Rtcm::print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) { unsigned int msg_number = 1020; + unsigned int glonass_gnav_alm_health = 0; + unsigned int glonass_gnav_alm_health_ind = 0; + unsigned int fifth_str_additional_data_ind = 1; Rtcm::set_DF002(msg_number); Rtcm::set_DF038(glonass_gnav_eph); Rtcm::set_DF040(glonass_gnav_eph); - Rtcm::set_DF104(glonass_gnav_eph); - Rtcm::set_DF105(glonass_gnav_eph); + Rtcm::set_DF104(glonass_gnav_alm_health); + Rtcm::set_DF105(glonass_gnav_alm_health_ind); Rtcm::set_DF106(glonass_gnav_eph); Rtcm::set_DF107(glonass_gnav_eph); Rtcm::set_DF108(glonass_gnav_eph); @@ -1743,11 +1746,11 @@ std::string Rtcm::print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) Rtcm::set_DF128(glonass_gnav_eph); Rtcm::set_DF129(glonass_gnav_eph); Rtcm::set_DF130(glonass_gnav_eph); - Rtcm::set_DF131(glonass_gnav_eph); - Rtcm::set_DF132(glonass_gnav_eph); - Rtcm::set_DF133(glonass_gnav_eph); - Rtcm::set_DF134(glonass_gnav_eph); - Rtcm::set_DF135(glonass_gnav_eph); + Rtcm::set_DF131(fifth_str_additional_data_ind); + Rtcm::set_DF132(glonass_gnav_utc_model); + Rtcm::set_DF133(glonass_gnav_utc_model); + Rtcm::set_DF134(glonass_gnav_utc_model); + Rtcm::set_DF135(glonass_gnav_utc_model); Rtcm::set_DF136(glonass_gnav_eph); std::string data; @@ -1804,10 +1807,13 @@ std::string Rtcm::print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) } -int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glonass_gnav_eph) +int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glonass_gnav_eph, Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) { // Convert message to binary std::string message_bin = Rtcm::binary_data_to_bin(message); + int glonass_gnav_alm_health; + int glonass_gnav_alm_health_ind; + int fifth_str_additional_data_ind; if(!Rtcm::check_CRC(message) ) { @@ -1845,20 +1851,21 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.i_satellite_freq_channel = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); index += 5; - glonass_gnav_eph.i_almanac_health = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + glonass_gnav_alm_health = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; - glonass_gnav_eph.i_alamanac_health_indi = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + glonass_gnav_alm_health_ind = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; glonass_gnav_eph.d_P_1 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); index += 2; - glonass_gnav_eph.d_t_k = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 1)))*30 - static_cast(Rtcm::bin_to_int(message_bin.substr(index, 5)))*3600 + - static_cast(Rtcm::bin_to_int(message_bin.substr(index, 6)))*60 - ; - index += 12; + glonass_gnav_eph.d_t_k += static_cast(Rtcm::bin_to_int(message_bin.substr(index, 5)))*3600; + index += 5; + glonass_gnav_eph.d_t_k += static_cast(Rtcm::bin_to_int(message_bin.substr(index, 6)))*60; + index += 6; + glonass_gnav_eph.d_t_k += static_cast(Rtcm::bin_to_int(message_bin.substr(index, 1)))*30; + index += 1; glonass_gnav_eph.d_B_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; @@ -1906,7 +1913,7 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.d_P = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); index += 2; - glonass_gnav_eph.d_l_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + glonass_gnav_eph.d_l3rd_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; glonass_gnav_eph.d_tau_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 22)))* TWO_N30; @@ -1930,22 +1937,22 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.d_M = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); index += 2; - glonass_gnav_eph.d_i_0 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + fifth_str_additional_data_ind = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; - glonass_gnav_eph.d_N_A = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); + glonass_gnav_utc_model.d_N_A = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); index += 11; - glonass_gnav_eph.d_tau_c = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 32)))* TWO_N31; + glonass_gnav_utc_model.d_tau_c = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 32)))* TWO_N31; index += 32; - glonass_gnav_eph.d_N_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + glonass_gnav_utc_model.d_N_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); index += 5; - glonass_gnav_eph.d_tau_gps = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 22)))* TWO_N30; + glonass_gnav_utc_model.d_tau_gps = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 22)))* TWO_N30; index += 22; - glonass_gnav_eph.d_l_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + glonass_gnav_eph.d_l5th_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); return 0; } @@ -2247,8 +2254,9 @@ std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1071; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1071; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1081; if(gal_eph.i_satellite_PRN != 0) msg_number = 1091; - if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -2293,9 +2301,10 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, bool divergence_free, bool more_messages) { + std::string sys(&observables.second.System, 1); Rtcm::set_DF002(msg_number); Rtcm::set_DF003(ref_id); - Rtcm::set_DF004(obs_time); + Rtcm::set_DF393(more_messages); Rtcm::set_DF409(0); // Issue of Data Station. 0: not utilized std::bitset<7> DF001_ = std::bitset<7>("0000000"); @@ -2308,7 +2317,20 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, Rtcm::set_DF395(observables); std::string header = DF002.to_string() + DF003.to_string(); - header += DF004.to_string(); + // GNSS Epoch Time Specific to each constellation + if((sys.compare("R") == 0 )) + { + // GLONASS Epoch Time + Rtcm::set_DF034(obs_time); + header += DF034.to_string(); + } + else + { + // GPS, Galileo Epoch Time + Rtcm::set_DF004(obs_time); + header += DF004.to_string(); + } + header = header + DF393.to_string() + DF409.to_string() + DF001_.to_string() + @@ -2413,8 +2435,9 @@ std::string Rtcm::print_MSM_2( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1072; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1072; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1082; if(gal_eph.i_satellite_PRN != 0) msg_number = 1092; - if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -2479,7 +2502,7 @@ std::string Rtcm::get_MSM_2_content_signal_data(const Gps_Ephemeris & ephNAV, for(unsigned int cell = 0; cell < Ncells ; cell++) { Rtcm::set_DF401(ordered_by_PRN_pos.at( cell ).second); - Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); + Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, ephGNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF420(ordered_by_PRN_pos.at( cell ).second); first_data_type += DF401.to_string(); second_data_type += DF402.to_string(); @@ -2514,8 +2537,9 @@ std::string Rtcm::print_MSM_3( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1073; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1073; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1083; if(gal_eph.i_satellite_PRN != 0) msg_number = 1093; - if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -2582,7 +2606,7 @@ std::string Rtcm::get_MSM_3_content_signal_data(const Gps_Ephemeris & ephNAV, { Rtcm::set_DF400(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF401(ordered_by_PRN_pos.at( cell ).second); - Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); + Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, ephGNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF420(ordered_by_PRN_pos.at( cell ).second); first_data_type += DF400.to_string(); second_data_type += DF401.to_string(); @@ -2617,8 +2641,9 @@ std::string Rtcm::print_MSM_4( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1074; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1074; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1084; if(gal_eph.i_satellite_PRN != 0) msg_number = 1094; - if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -2726,7 +2751,7 @@ std::string Rtcm::get_MSM_4_content_signal_data(const Gps_Ephemeris & ephNAV, { Rtcm::set_DF400(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF401(ordered_by_PRN_pos.at( cell ).second); - Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); + Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, ephGNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF420(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF403(ordered_by_PRN_pos.at( cell ).second); first_data_type += DF400.to_string(); @@ -2763,8 +2788,9 @@ std::string Rtcm::print_MSM_5( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1075; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1075; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1085; if(gal_eph.i_satellite_PRN != 0) msg_number = 1095; - if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -2879,7 +2905,7 @@ std::string Rtcm::get_MSM_5_content_signal_data(const Gps_Ephemeris & ephNAV, { Rtcm::set_DF400(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF401(ordered_by_PRN_pos.at( cell ).second); - Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); + Rtcm::set_DF402(ephNAV, ephCNAV, ephFNAV, ephGNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF420(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF403(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF404(ordered_by_PRN_pos.at( cell ).second); @@ -2919,8 +2945,9 @@ std::string Rtcm::print_MSM_6( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1076; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1076; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1086; if(gal_eph.i_satellite_PRN != 0) msg_number = 1096; - if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) ||(gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -2988,7 +3015,7 @@ std::string Rtcm::get_MSM_6_content_signal_data(const Gps_Ephemeris & ephNAV, { Rtcm::set_DF405(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF406(ordered_by_PRN_pos.at( cell ).second); - Rtcm::set_DF407(ephNAV, ephCNAV, ephFNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); + Rtcm::set_DF407(ephNAV, ephCNAV, ephFNAV, ephGNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF420(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF408(ordered_by_PRN_pos.at( cell ).second); first_data_type += DF405.to_string(); @@ -3026,8 +3053,9 @@ std::string Rtcm::print_MSM_7( const Gps_Ephemeris & gps_eph, unsigned int msg_number = 0; if(gps_eph.i_satellite_PRN != 0) msg_number = 1077; if(gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1077; + if(glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1087; if(gal_eph.i_satellite_PRN != 0) msg_number = 1097; - if(((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0) ) && (gal_eph.i_satellite_PRN != 0)) + if(((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0) ) && (glo_gnav_eph.i_satellite_PRN != 0) && (gal_eph.i_satellite_PRN != 0)) { LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? } @@ -3096,7 +3124,7 @@ std::string Rtcm::get_MSM_7_content_signal_data(const Gps_Ephemeris & ephNAV, { Rtcm::set_DF405(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF406(ordered_by_PRN_pos.at( cell ).second); - Rtcm::set_DF407(ephNAV, ephCNAV, ephFNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); + Rtcm::set_DF407(ephNAV, ephCNAV, ephFNAV, ephGNAV, obs_time, ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF420(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF408(ordered_by_PRN_pos.at( cell ).second); Rtcm::set_DF404(ordered_by_PRN_pos.at( cell ).second); @@ -3724,7 +3752,7 @@ int Rtcm::set_DF034(double obs_time) if(tk > 86400999) { LOG(WARNING) << "To large GLONASS Epoch Time (tk)! Set to the last millisecond of the day"; - tow = 86400999; + tk = 86400999; } DF034 = std::bitset<27>(tk); return 0; @@ -3798,10 +3826,23 @@ int Rtcm::set_DF039(bool code_indicator) } -int Rtcm::set_DF040(unsigned short frequency_channel_number) +int Rtcm::set_DF040(unsigned int frequency_channel_number) +{ + unsigned int freq_ = frequency_channel_number; + if(freq_ > 20) + { + LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; + } + + DF040 = std::bitset<5>(freq_); + return 0; +} + + +int Rtcm::set_DF040(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { unsigned int freq_ = glonass_gnav_eph.i_satellite_freq_channel; - if(prn_ > 20) + if(freq_ > 20) { LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; } @@ -4186,6 +4227,264 @@ int Rtcm::set_DF103(const Gps_Ephemeris & gps_eph) } +int Rtcm::set_DF104(unsigned int glonass_gnav_alm_health) +{ + DF104 = std::bitset<1>(glonass_gnav_alm_health); + return 0; +} + + +int Rtcm::set_DF105(unsigned int glonass_gnav_alm_health_ind) +{ + DF105 = std::bitset<1>(glonass_gnav_alm_health_ind); + return 0; +} + + +int Rtcm::set_DF106(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int P_1 = static_cast(std::round(glonass_gnav_eph.d_P_1)); + DF106 = std::bitset<2>(P_1); + return 0; +} + + +int Rtcm::set_DF107(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + // TODO Need to fix this here, bit position has a given order + DF107 = std::bitset<12>(glonass_gnav_eph.d_t_k); + return 0; +} + + +int Rtcm::set_DF108(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + DF108 = std::bitset<1>(glonass_gnav_eph.d_B_n); + return 0; +} + + +int Rtcm::set_DF109(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + DF109 = std::bitset<1>(glonass_gnav_eph.d_P_2); + return 0; +} + + +int Rtcm::set_DF110(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int t_b = static_cast(std::round(glonass_gnav_eph.d_t_b)); + DF110 = std::bitset<7>(t_b); + return 0; +} + + +int Rtcm::set_DF111(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int VXn = static_cast(std::round(glonass_gnav_eph.d_VXn/TWO_N20)); + DF111 = std::bitset<24>(VXn); + return 0; +} + + +int Rtcm::set_DF112(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int Xn = static_cast(std::round(glonass_gnav_eph.d_Xn/TWO_N11)); + DF112 = std::bitset<27>(Xn); + return 0; +} + + +int Rtcm::set_DF113(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int AXn = static_cast(std::round(glonass_gnav_eph.d_AXn/TWO_N11)); + DF113 = std::bitset<5>(AXn); + return 0; +} + + +int Rtcm::set_DF114(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int VYn = static_cast(std::round(glonass_gnav_eph.d_VYn/TWO_N20)); + DF114 = std::bitset<24>(VYn); + return 0; +} + + +int Rtcm::set_DF115(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int Yn = static_cast(std::round(glonass_gnav_eph.d_Yn/TWO_N11)); + DF115 = std::bitset<27>(Yn); + return 0; +} + + +int Rtcm::set_DF116(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int AYn = static_cast(std::round(glonass_gnav_eph.d_AYn/TWO_N11)); + DF116 = std::bitset<5>(AYn); + return 0; +} + + +int Rtcm::set_DF117(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int VZn = static_cast(std::round(glonass_gnav_eph.d_VZn/TWO_N20)); + DF117 = std::bitset<24>(VZn); + return 0; +} + + +int Rtcm::set_DF118(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int Zn = static_cast(std::round(glonass_gnav_eph.d_Zn/TWO_N11)); + DF118 = std::bitset<27>(Zn); + return 0; +} + + +int Rtcm::set_DF119(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int AZn = static_cast(std::round(glonass_gnav_eph.d_AZn/TWO_N11)); + DF119 = std::bitset<5>(AZn); + return 0; +} + + +int Rtcm::set_DF120(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int P3 = static_cast(std::round(glonass_gnav_eph.d_P_3/TWO_N11)); + DF120 = std::bitset<1>(P3); + return 0; +} + + +int Rtcm::set_DF121(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int gamma_n = static_cast(std::round(glonass_gnav_eph.d_gamma_n/TWO_N40)); + DF121 = std::bitset<11>(gamma_n); + return 0; +} + + +int Rtcm::set_DF122(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int P = static_cast(std::round(glonass_gnav_eph.d_P)); + DF122 = std::bitset<2>(P); + return 0; +} + + +int Rtcm::set_DF123(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int ln = static_cast((glonass_gnav_eph.d_l3rd_n)); + DF123 = std::bitset<1>(ln); + return 0; +} + + +int Rtcm::set_DF124(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int tau_n = static_cast(std::round(glonass_gnav_eph.d_tau_n/TWO_N30)); + DF124 = std::bitset<22>(tau_n); + return 0; +} + + +int Rtcm::set_DF125(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + int delta_tau_n = static_cast(std::round(glonass_gnav_eph.d_Delta_tau_n)); + DF125 = std::bitset<5>(delta_tau_n); + return 0; +} + + +int Rtcm::set_DF126(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int ecc = static_cast(std::round(glonass_gnav_eph.d_E_n)); + DF126 = std::bitset<5>(ecc); + return 0; +} + + +int Rtcm::set_DF127(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int P4= static_cast(std::round(glonass_gnav_eph.d_P_4)); + DF127 = std::bitset<1>(P4); + return 0; +} + + +int Rtcm::set_DF128(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int F_t = static_cast(std::round(glonass_gnav_eph.d_F_T)); + DF128 = std::bitset<4>(F_t); + return 0; +} + + +int Rtcm::set_DF129(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int N_t = static_cast(std::round(glonass_gnav_eph.d_N_T)); + DF129 = std::bitset<11>(N_t); + return 0; +} + + +int Rtcm::set_DF130(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int M = static_cast(std::round(glonass_gnav_eph.d_M)); + DF130 = std::bitset<2>(M); + return 0; +} + + +int Rtcm::set_DF131(unsigned int fifth_str_additional_data_ind) +{ + unsigned int fith_str_data = static_cast(fifth_str_additional_data_ind); + DF131 = std::bitset<1>(fith_str_data); + return 0; +} + + +int Rtcm::set_DF132(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) +{ + unsigned int N_A = static_cast(std::round(glonass_gnav_utc_model.d_N_A)); + DF132 = std::bitset<11>(N_A); + return 0; +} + + +int Rtcm::set_DF133(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) +{ + int tau_c = static_cast(std::round(glonass_gnav_utc_model.d_tau_c/TWO_N31)); + DF133 = std::bitset<32>(tau_c); + return 0; +} + + +int Rtcm::set_DF134(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) +{ + unsigned int N_4 = static_cast(std::round(glonass_gnav_utc_model.d_N_4)); + DF134 = std::bitset<11>(N_4); + return 0; +} + + +int Rtcm::set_DF135(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) +{ + int tau_gps = static_cast(std::round(glonass_gnav_utc_model.d_tau_gps)/TWO_N30); + DF135 = std::bitset<22>(tau_gps); + return 0; +} + + +int Rtcm::set_DF136(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +{ + unsigned int l_n = static_cast(std::round(glonass_gnav_eph.d_l5th_n)); + DF136 = std::bitset<1>(l_n); + return 0; +} int Rtcm::set_DF137(const Gps_Ephemeris & gps_eph) @@ -4526,6 +4825,16 @@ int Rtcm::set_DF395(const std::map & gnss_synchro) mask_position = 32 - 16; DF395.set(mask_position, true); } + if ((sig.compare("1C") == 0) && (sys.compare("R") == 0 ) ) + { + mask_position = 32 - 2; + DF395.set(mask_position, true); + } + if ((sig.compare("2C") == 0) && (sys.compare("R") == 0 ) ) + { + mask_position = 32 - 8; + DF395.set(mask_position, true); + } } return 0; @@ -4786,27 +5095,38 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro) double lambda = 0.0; std::string sig_(gnss_synchro.Signal); std::string sig = sig_.substr(0,2); + std::string sys(&gnss_synchro.System, 1); - if (sig.compare("1C") == 0 ) + if ((sig.compare("1C") == 0) && (sys.compare("G") == 0 )) { lambda = GPS_C_m_s / GPS_L1_FREQ_HZ; } - if (sig.compare("2S") == 0 ) + if ((sig.compare("2S")) == 0 && (sys.compare("G") == 0 )) { lambda = GPS_C_m_s / GPS_L2_FREQ_HZ; } - if (sig.compare("5X") == 0 ) + if ((sig.compare("5X")) == 0 && (sys.compare("E") == 0 )) { lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ; } - if (sig.compare("1B") == 0 ) + if ((sig.compare("1B")) == 0 && (sys.compare("E") == 0 )) { lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ; } - if (sig.compare("7X") == 0 ) + if ((sig.compare("7X")) == 0 && (sys.compare("E") == 0 )) { lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ; } + if ((sig.compare("1C") == 0) && (sys.compare("R") == 0 )) + { + // TODO Need to add slot number and freq number to gnss_syncro + lambda = GLONASS_C_m_s / (GLONASS_L1_FREQ_HZ); + } + if ((sig.compare("2C") == 0) && (sys.compare("R") == 0 )) + { + // TODO Need to add slot number and freq number to gnss_syncro + lambda = GLONASS_C_m_s / (GLONASS_L2_FREQ_HZ); + } phrng_m = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI ) * lambda - rough_range_m; @@ -4837,23 +5157,33 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro) } -int Rtcm::set_DF402(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const Gnss_Synchro & gnss_synchro) +int Rtcm::set_DF402(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const Gnss_Synchro & gnss_synchro) { unsigned int lock_time_period_s = 0; unsigned int lock_time_indicator; std::string sig_(gnss_synchro.Signal); - if(sig_.compare("1C")) + std::string sys(&gnss_synchro.System, 1); + if ((sig_.compare("1C") == 0) && (sys.compare("G") == 0 )) { lock_time_period_s = Rtcm::lock_time(ephNAV, obs_time, gnss_synchro); } - if(sig_.compare("2S")) + if ((sig_.compare("2S") == 0) && (sys.compare("G") == 0 )) { lock_time_period_s = Rtcm::lock_time(ephCNAV, obs_time, gnss_synchro); } + // TODO Should add system for galileo satellites if(sig_.compare("1B") || sig_.compare("5X") || sig_.compare("7X") || sig_.compare("8X")) { lock_time_period_s = Rtcm::lock_time(ephFNAV, obs_time, gnss_synchro); } + if ((sig_.compare("1C") == 0) && (sys.compare("R") == 0 )) + { + lock_time_period_s = Rtcm::lock_time(ephGNAV, obs_time, gnss_synchro); + } + if ((sig_.compare("2C") == 0) && (sys.compare("R") == 0 )) + { + lock_time_period_s = Rtcm::lock_time(ephGNAV, obs_time, gnss_synchro); + } lock_time_indicator = Rtcm::msm_lock_time_indicator(lock_time_period_s); DF402 = std::bitset<4>(lock_time_indicator); return 0; @@ -4875,28 +5205,38 @@ int Rtcm::set_DF404(const Gnss_Synchro & gnss_synchro) std::string sig_(gnss_synchro.Signal); std::string sig = sig_.substr(0,2); int fine_phaserange_rate; + std::string sys_(&gnss_synchro.System, 1); - if (sig.compare("1C") == 0 ) + if ((sig_.compare("1C") == 0) && (sys_.compare("G") == 0 )) { lambda = GPS_C_m_s / GPS_L1_FREQ_HZ; } - if (sig.compare("2S") == 0 ) + if ((sig_.compare("2S") == 0) && (sys_.compare("G") == 0 )) { lambda = GPS_C_m_s / GPS_L2_FREQ_HZ; } - if (sig.compare("5X") == 0 ) + if ((sig_.compare("5X") == 0) && (sys_.compare("E") == 0 )) { lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ; } - if (sig.compare("1B") == 0 ) + if ((sig_.compare("1B") == 0) && (sys_.compare("E") == 0 )) { lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ; } - if (sig.compare("7X") == 0 ) + if ((sig_.compare("7X") == 0 ) && (sys_.compare("E") == 0 )) { lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ; } - + if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) + { + //TODO Need to add slot number and freq number to gnss syncro + lambda = GLONASS_C_m_s / (GLONASS_L1_FREQ_HZ); + } + if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) + { + //TODO Need to add slot number and freq number to gnss syncro + lambda = GLONASS_C_m_s / (GLONASS_L2_FREQ_HZ); + } double rough_phase_range_rate = std::round(- gnss_synchro.Carrier_Doppler_hz * lambda ); double phrr = (- gnss_synchro.Carrier_Doppler_hz * lambda - rough_phase_range_rate); @@ -4953,29 +5293,39 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro) double phrng_m; double lambda = 0.0; std::string sig_(gnss_synchro.Signal); - std::string sig = sig_.substr(0,2); + sig_ = sig_.substr(0,2); + std::string sys_(&gnss_synchro.System, 1); - if (sig.compare("1C") == 0 ) + if ((sig_.compare("1C") == 0) && (sys_.compare("G") == 0 ) ) { lambda = GPS_C_m_s / GPS_L1_FREQ_HZ; } - if (sig.compare("2S") == 0 ) + if ((sig_.compare("2S") == 0) && (sys_.compare("G") == 0 ) ) { lambda = GPS_C_m_s / GPS_L2_FREQ_HZ; } - if (sig.compare("5X") == 0 ) + if ((sig_.compare("5X") == 0) && (sys_.compare("E") == 0 ) ) { lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ; } - if (sig.compare("1B") == 0 ) + if ((sig_.compare("1B") == 0) && (sys_.compare("E") == 0 ) ) { lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ; } - if (sig.compare("7X") == 0 ) + if ((sig_.compare("7X") == 0 ) && (sys_.compare("E") == 0 ) ) { lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ; } - + if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) + { + //TODO Need to add slot number and freq number to gnss syncro + lambda = GLONASS_C_m_s / (GLONASS_L1_FREQ_HZ); + } + if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) + { + //TODO Need to add slot number and freq number to gnss syncro + lambda = GLONASS_C_m_s / (GLONASS_L2_FREQ_HZ); + } phrng_m = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI ) * lambda - rough_range_m; /* Substract phase - pseudorange integer cycle offset */ @@ -5005,24 +5355,33 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro) } -int Rtcm::set_DF407(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const Gnss_Synchro & gnss_synchro) +int Rtcm::set_DF407(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const Gnss_Synchro & gnss_synchro) { unsigned int lock_time_indicator; unsigned int lock_time_period_s = 0; std::string sig_(gnss_synchro.Signal); - if(sig_.compare("1C")) + std::string sys_(&gnss_synchro.System, 1); + if((sig_.compare("1C")) && (sys_.compare("G") == 0 )) { lock_time_period_s = Rtcm::lock_time(ephNAV, obs_time, gnss_synchro); } - if(sig_.compare("2S")) + if((sig_.compare("2S")) && (sys_.compare("G") == 0 )) { lock_time_period_s = Rtcm::lock_time(ephCNAV, obs_time, gnss_synchro); } - if(sig_.compare("1B") || sig_.compare("5X") || sig_.compare("7X") || sig_.compare("8X")) + if((sig_.compare("1B") || sig_.compare("5X") || sig_.compare("7X") || sig_.compare("8X") ) && (sys_.compare("E") == 0 )) { lock_time_period_s = Rtcm::lock_time(ephFNAV, obs_time, gnss_synchro); } + if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) + { + lock_time_period_s = Rtcm::lock_time(ephGNAV, obs_time, gnss_synchro); + } + if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) + { + lock_time_period_s = Rtcm::lock_time(ephGNAV, obs_time, gnss_synchro); + } lock_time_indicator = Rtcm::msm_extended_lock_time_indicator(lock_time_period_s); DF407 = std::bitset<10>(lock_time_indicator); return 0; diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index d3b84beb1..ed4369923 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -158,7 +158,7 @@ public: * \params observables Set of observables as defined by the platform * \return string with message contents */ - std::string print_MT1011(Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); + std::string print_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); /*! * \brief Prints Extended L1&L2 GLONASS RTK Observables * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. @@ -168,7 +168,7 @@ public: * \params observables Set of observables as defined by the platform * \return string with message contents */ - std::string print_MT1012(Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); + std::string print_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); /*! * \brief Prints message type 1019 (GPS Ephemeris), should be broadcast in the event that @@ -185,18 +185,20 @@ public: * \brief Prints message type 1020 (GLONASS Ephemeris). * \note Code added as part of GSoC 2017 program * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param glonass_gnav_utc_model GLONASS GNAV Clock Information * \return Returns message type as a string type */ - std::string print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + std::string print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); /*! * \brief Verifies and reads messages of type 1020 (GLONASS Ephemeris). * \note Code added as part of GSoC 2017 program * \param message Message to read as a string type * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param glonass_gnav_utc_model GLONASS GNAV Clock Information * \return Returns 1 if anything goes wrong, 0 otherwise. */ - int read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glonass_gnav_eph, Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); /*! * \brief Prints message type 1029 (Unicode Text String) @@ -1023,7 +1025,7 @@ private: int set_DF036(bool divergence_free_smoothing_indicator); std::bitset<3> DF037; //!< GLONASS Smoothing Interval - int set_DF037(unsigned short smoothing_interval); + int set_DF037(short int smoothing_interval); std::bitset<6> DF038; //!< GLONASS Satellite ID (Satellite Slot Number) int set_DF038(const Gnss_Synchro & gnss_synchro); @@ -1161,10 +1163,10 @@ private: int set_DF103(const Gps_Ephemeris & gps_eph); std::bitset<1> DF104; //!< GLONASS Almanac Health - int set_DF104(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF104(unsigned int glonass_gnav_alm_health); std::bitset<1> DF105; //!< GLONASS Almanac Health Availability Indicator - int set_DF105(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF105(unsigned int glonass_gnav_alm_health_ind); std::bitset<2> DF106; //!< GLONASS P1 Word int set_DF106(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); @@ -1235,26 +1237,26 @@ private: std::bitset<4> DF128; //!< GLONASS F_T int set_DF128(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); - std::bitset<1> DF129; //!< GLONASS N_T + std::bitset<11> DF129; //!< GLONASS N_T int set_DF129(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); std::bitset<2> DF130; //!< GLONASS M int set_DF130(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); std::bitset<1> DF131; //!< GLONASS Availability of additional data - int set_DF131(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF131(unsigned int fifth_str_additional_data_ind); std::bitset<11> DF132; //!< GLONASS N_A - int set_DF132(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF132(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); std::bitset<32> DF133; //!< GLONASS TAU_C - int set_DF133(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF133(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); std::bitset<5> DF134; //!< GLONASS N_4 - int set_DF134(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF134(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); std::bitset<22> DF135; //!< GLONASS TAU_GPS - int set_DF135(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); + int set_DF135(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); std::bitset<1> DF136; //!< GLONASS L_N (FIFTH STRING) int set_DF136(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); @@ -1387,7 +1389,7 @@ private: int set_DF401(const Gnss_Synchro & gnss_synchro); std::bitset<4> DF402; - int set_DF402(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const Gnss_Synchro & gnss_synchro); + int set_DF402(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const Gnss_Synchro & gnss_synchro); std::bitset<6> DF403; int set_DF403(const Gnss_Synchro & gnss_synchro); @@ -1402,7 +1404,7 @@ private: int set_DF406(const Gnss_Synchro & gnss_synchro); std::bitset<10> DF407; - int set_DF407(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, double obs_time, const Gnss_Synchro & gnss_synchro); + int set_DF407(const Gps_Ephemeris & ephNAV, const Gps_CNAV_Ephemeris & ephCNAV, const Galileo_Ephemeris & ephFNAV, const Glonass_Gnav_Ephemeris & ephGNAV, double obs_time, const Gnss_Synchro & gnss_synchro); std::bitset<10> DF408; int set_DF408(const Gnss_Synchro & gnss_synchro); From f720ddf20876ad7d1c7b504719e8d5c7a48127bc Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 13 Aug 2017 09:58:35 -0700 Subject: [PATCH 31/48] Adding RTCM messages for GLONASS support Adds code for RTCM in GLONASS processing. The commit includes legacy messages (MS1009-MS1012) and integrated messages for GNSS fields. A couple of bug fixes are included and addition of ephemeris fields is completed. --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 20 +++++------ src/algorithms/PVT/libs/rinex_printer.cc | 9 ++++- src/algorithms/PVT/libs/rtcm_printer.cc | 12 +++---- src/algorithms/PVT/libs/rtcm_printer.h | 35 ++++++++++--------- src/core/system_parameters/MATH_CONSTANTS.h | 1 + .../glonass_gnav_navigation_message.h | 4 +-- src/core/system_parameters/rtcm.cc | 34 ++++++++++-------- 7 files changed, 66 insertions(+), 49 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index e28e35b01..cbc13813a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1116,7 +1116,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1135,7 +1135,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1156,7 +1156,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) ) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1214,14 +1214,14 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } if(flag_write_RTCM_MSM_output == true) { if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1241,7 +1241,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -1257,7 +1257,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -1273,7 +1273,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -1326,12 +1326,12 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0) ) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 7c51b8c95..362b6f289 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -410,6 +410,7 @@ std::string Rinex_Printer::getLocalTime() void Rinex_Printer::rinex_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { + if(glonass_gnav_almanac.i_satellite_freq_channel){} std::string line; stringVersion = "3.02"; version = 3; @@ -508,6 +509,7 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Glonass_Gnav_Utc_M void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { + if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning std::string line; stringVersion = "3.02"; version = 3; @@ -630,6 +632,7 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { + if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning std::string line; stringVersion = "3.02"; version = 3; @@ -1389,6 +1392,7 @@ void Rinex_Printer::rinex_sbs_header(std::fstream& out) void Rinex_Printer::update_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { + if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning std::vector data; std::string line_aux; @@ -1935,6 +1939,7 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { + if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning std::vector data; std::string line_aux; @@ -2041,6 +2046,7 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { + if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning std::vector data; std::string line_aux; @@ -3031,6 +3037,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map::const_iterator glonass_gnav_ephemeris_iter; @@ -5163,7 +5170,7 @@ void Rinex_Printer::update_obs_header(std::fstream& out, const Glonass_Gnav_Utc_ { if(utc_model.d_N_4) { - + } } diff --git a/src/algorithms/PVT/libs/rtcm_printer.cc b/src/algorithms/PVT/libs/rtcm_printer.cc index 13873251d..9bd55b6c1 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.cc +++ b/src/algorithms/PVT/libs/rtcm_printer.cc @@ -202,17 +202,17 @@ bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_ } -bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables) +bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables) { - std::string m1011 = rtcm->print_MT1011(glonass_gnav_eph, obs_time, observables, station_id); + std::string m1011 = rtcm->print_MT1011(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id); Rtcm_Printer::Print_Message(m1011); return true; } -bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables) +bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables) { - std::string m1012 = rtcm->print_MT1012(glonass_gnav_eph, obs_time, observables, station_id); + std::string m1012 = rtcm->print_MT1012(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id); Rtcm_Printer::Print_Message(m1012); return true; } @@ -226,9 +226,9 @@ bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph) } -bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) +bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) { - std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph); + std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); Rtcm_Printer::Print_Message(m1020); return true; } diff --git a/src/algorithms/PVT/libs/rtcm_printer.h b/src/algorithms/PVT/libs/rtcm_printer.h index e2622d678..21fdaeddf 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.h +++ b/src/algorithms/PVT/libs/rtcm_printer.h @@ -63,9 +63,9 @@ public: * \brief Prints L1-Only GLONASS RTK Observables * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return true or false upon operation success */ bool Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); @@ -73,9 +73,9 @@ public: * \brief Prints Extended L1-Only GLONASS RTK Observables * \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return true or false upon operation success */ bool Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); @@ -83,22 +83,24 @@ public: * \brief Prints L1&L2 GLONASS RTK Observables * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_ephL1 GLONASS L1 GNAV Broadcast Ephemeris for satellite + * \param glonass_gnav_ephL2 GLONASS L2 GNAV Broadcast Ephemeris for satellite + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return true or false upon operation success */ - bool Print_Rtcm_MT1011(Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); + bool Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables); /*! * \brief Prints Extended L1&L2 GLONASS RTK Observables * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_ephL1 GLONASS L1 GNAV Broadcast Ephemeris for satellite + * \param glonass_gnav_ephL2 GLONASS L2 GNAV Broadcast Ephemeris for satellite + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return true or false upon operation success */ - bool Print_Rtcm_MT1012(Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables); + bool Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables); bool Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph); //(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; + if(glonass_gnav_alm_health){} //Avoid comiler warning glonass_gnav_alm_health_ind = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; + if(glonass_gnav_alm_health_ind){} //Avoid comiler warning glonass_gnav_eph.d_P_1 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); index += 2; @@ -1940,19 +1942,22 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon fifth_str_additional_data_ind = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; - glonass_gnav_utc_model.d_N_A = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); - index += 11; + if(fifth_str_additional_data_ind == true) + { + glonass_gnav_utc_model.d_N_A = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); + index += 11; - glonass_gnav_utc_model.d_tau_c = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 32)))* TWO_N31; - index += 32; + glonass_gnav_utc_model.d_tau_c = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 32)))* TWO_N31; + index += 32; - glonass_gnav_utc_model.d_N_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); - index += 5; + glonass_gnav_utc_model.d_N_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + index += 5; - glonass_gnav_utc_model.d_tau_gps = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 22)))* TWO_N30; - index += 22; + glonass_gnav_utc_model.d_tau_gps = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 22)))* TWO_N30; + index += 22; - glonass_gnav_eph.d_l5th_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + glonass_gnav_eph.d_l5th_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + } return 0; } @@ -2301,7 +2306,8 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, bool divergence_free, bool more_messages) { - std::string sys(&observables.second.System, 1); + std::map::const_iterator gnss_synchro_iter; + std::string sys(gnss_synchro_iter->second.System, 1); Rtcm::set_DF002(msg_number); Rtcm::set_DF003(ref_id); @@ -4466,7 +4472,7 @@ int Rtcm::set_DF133(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) int Rtcm::set_DF134(const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) { unsigned int N_4 = static_cast(std::round(glonass_gnav_utc_model.d_N_4)); - DF134 = std::bitset<11>(N_4); + DF134 = std::bitset<5>(N_4); return 0; } From bc8d0bbe64ac87759212beba5aa36ed11fa1fc11 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 13 Aug 2017 20:42:24 -0700 Subject: [PATCH 32/48] Adds unit test for RINEX and RTCM messages printer Adds set of unit test to check functionality of GLONASS processing for RINEX and RTCM messages. It also fixes a couple of bug fixes with formatting in existing code. --- .../glonass_gnav_ephemeris.h | 4 +- src/core/system_parameters/rtcm.cc | 50 ++++ .../pvt/rinex_printer_test.cc | 249 ++++++++++++++++++ .../signal-processing-blocks/pvt/rtcm_test.cc | 57 +++- 4 files changed, 350 insertions(+), 10 deletions(-) diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 153f5b5b1..3f8fa699c 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -95,8 +95,8 @@ public: double d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] double d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - double d_l3rd_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - double 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] + double d_l3rd_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + double 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] // Inmediate deliverables of ephemeris information //TODO check how freq channel is managed in gnav message. I think it is a number greater thn 0 diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 7e11062a8..3a82d5647 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -3310,6 +3310,13 @@ boost::posix_time::ptime Rtcm::compute_Galileo_time(const Galileo_Ephemeris & ep } +boost::posix_time::ptime Rtcm::compute_GLONASS_time(const Glonass_Gnav_Ephemeris & eph, double obs_time) const +{ + boost::posix_time::ptime p_time = eph.compute_GLONASS_time(obs_time); + return p_time; +} + + unsigned int Rtcm::lock_time(const Gps_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro) { unsigned int lock_time_in_seconds; @@ -3385,6 +3392,49 @@ unsigned int Rtcm::lock_time(const Galileo_Ephemeris & eph, double obs_time, con } +unsigned int Rtcm::lock_time(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro) +{ + unsigned int lock_time_in_seconds; + boost::posix_time::ptime current_time = Rtcm::compute_GLONASS_time(eph, obs_time); + + boost::posix_time::ptime last_lock_time; + std::string sig_(gnss_synchro.Signal); + if(sig_.compare("1C") == 0) + { + last_lock_time = Rtcm::glo_L1_last_lock_time[65 - gnss_synchro.PRN]; + } + if(sig_.compare("2C") == 0) + { + last_lock_time = Rtcm::glo_L2_last_lock_time[65 - gnss_synchro.PRN]; + } + + if(last_lock_time.is_not_a_date_time() )// || CHECK LLI!!......) + { + if(sig_.compare("1C") == 0) + { + Rtcm::glo_L1_last_lock_time[65 - gnss_synchro.PRN] = current_time; + } + if(sig_.compare("2C") == 0) + { + Rtcm::glo_L2_last_lock_time[65 - gnss_synchro.PRN] = current_time; + } + } + + boost::posix_time::time_duration lock_duration = current_time - current_time; + if(sig_.compare("1C") == 0) + { + lock_duration = current_time - Rtcm::glo_L1_last_lock_time[65 - gnss_synchro.PRN]; + } + if(sig_.compare("2C") == 0) + { + lock_duration = current_time - Rtcm::glo_L2_last_lock_time[65 - gnss_synchro.PRN]; + } + + lock_time_in_seconds = static_cast(lock_duration.total_seconds()); + return lock_time_in_seconds; +} + + unsigned int Rtcm::lock_time_indicator(unsigned int lock_time_period_s) { // Table 3.4-2 diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc index fdf1d7d2c..33ed3965c 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc @@ -87,6 +87,35 @@ TEST(RinexPrinterTest, GalileoObsHeader) } +TEST(RinexPrinterTest, GlonassObsHeader) +{ + std::string line_aux; + std::string line_str; + bool no_more_finds = false; + const Glonass_Gnav_Ephemeris eph = Glonass_Gnav_Ephemeris(); + + std::shared_ptr rp1; + rp1 = std::make_shared(); + rp1->rinex_obs_header(rp1->obsFile, eph, 0.0); + rp1->obsFile.seekp(0); + + while(!rp1->obsFile.eof()) + { + std::getline(rp1->obsFile, line_str); + if(!no_more_finds) + { + if (line_str.find("SYS / # / OBS TYPES", 59) != std::string::npos) + { + no_more_finds = true; + line_aux = std::string(line_str); + } + } + } + std::string expected_str("R 4 C1C L1C D1C S1C SYS / # / OBS TYPES "); + EXPECT_EQ(0, expected_str.compare(line_aux)); + if(remove(rp1->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; + line_aux.clear(); +} TEST(RinexPrinterTest, MixedObsHeader) @@ -133,6 +162,50 @@ TEST(RinexPrinterTest, MixedObsHeader) } +TEST(RinexPrinterTest, MixedObsHeaderGpsGlo) +{ + std::string line_aux; + std::string line_aux2; + std::string line_str; + bool no_more_finds = false; + const Glonass_Gnav_Ephemeris eph_glo = Glonass_Gnav_Ephemeris(); + const Gps_Ephemeris eph_gps = Gps_Ephemeris(); + + std::shared_ptr rp1; + rp1 = std::make_shared(); + rp1->rinex_obs_header(rp1->obsFile, eph_gps, eph_glo, 0.0, "1C"); + rp1->obsFile.seekp(0); + int systems_found = 0; + + while(!rp1->obsFile.eof()) + { + std::getline(rp1->obsFile, line_str); + if(!no_more_finds) + { + if (line_str.find("SYS / # / OBS TYPES", 59) != std::string::npos) + { + systems_found++; + if(systems_found == 1) + { + line_aux = std::string(line_str); + } + if(systems_found == 2) + { + line_aux2 = std::string(line_str); + no_more_finds = true; + } + } + } + } + + std::string expected_str("G 4 C1C L1C D1C S1C SYS / # / OBS TYPES "); + std::string expected_str2("R 8 C1C L1C D1C S1C SYS / # / OBS TYPES "); + EXPECT_EQ(0, expected_str.compare(line_aux)); + EXPECT_EQ(0, expected_str2.compare(line_aux2)); + if(remove(rp1->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; +} + + TEST(RinexPrinterTest, GalileoObsLog) { std::string line_aux; @@ -201,6 +274,74 @@ TEST(RinexPrinterTest, GalileoObsLog) } +TEST(RinexPrinterTest, GlonassObsLog) +{ + std::string line_aux; + std::string line_str; + bool no_more_finds = false; + const Glonass_Gnav_Ephemeris eph = Glonass_Gnav_Ephemeris(); + + std::shared_ptr rp; + rp = std::make_shared(); + rp->rinex_obs_header(rp->obsFile, eph, 0.0); + + std::map gnss_pseudoranges_map; + + Gnss_Synchro gs1 = Gnss_Synchro(); + Gnss_Synchro gs2 = Gnss_Synchro(); + Gnss_Synchro gs3 = Gnss_Synchro(); + Gnss_Synchro gs4 = Gnss_Synchro(); + + std::string sys = "R"; + gs1.System = *sys.c_str(); + gs2.System = *sys.c_str(); + gs3.System = *sys.c_str(); + gs4.System = *sys.c_str(); + + std::string sig = "1C"; + std::memcpy((void*)gs1.Signal, sig.c_str(), 3); + std::memcpy((void*)gs2.Signal, sig.c_str(), 3); + std::memcpy((void*)gs3.Signal, sig.c_str(), 3); + std::memcpy((void*)gs4.Signal, sig.c_str(), 3); + + gs1.PRN = 3; + gs2.PRN = 8; + gs3.PRN = 10; + gs4.PRN = 22; + + gs4.Pseudorange_m = 22000000; + gs4.Carrier_phase_rads = 23.4; + gs4.Carrier_Doppler_hz = 1534; + gs4.CN0_dB_hz = 42; + + gnss_pseudoranges_map.insert( std::pair(1,gs1) ); + gnss_pseudoranges_map.insert( std::pair(2,gs2) ); + gnss_pseudoranges_map.insert( std::pair(3,gs3) ); + gnss_pseudoranges_map.insert( std::pair(4,gs4) ); + + rp->log_rinex_obs(rp->obsFile, eph, 0.0, gnss_pseudoranges_map); + rp->obsFile.seekp(0); + + while(!rp->obsFile.eof()) + { + std::getline(rp->obsFile, line_str); + if(!no_more_finds) + { + if (line_str.find("R22", 0) != std::string::npos) + { + no_more_finds = true; + line_aux = std::string(line_str); + } + } + } + + std::string expected_str("R22 22000000.000 7 3.724 7 1534.000 7 42.000 "); + EXPECT_EQ(0, expected_str.compare(line_aux)); + + if(remove(rp->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; +} + + TEST(RinexPrinterTest, GpsObsLogDualBand) { std::string line_aux; @@ -473,3 +614,111 @@ TEST(RinexPrinterTest, MixedObsLog) if(remove(rp->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; } + + +TEST(RinexPrinterTest, MixedObsLogGpsGlo) +{ + std::string line_aux; + std::string line_str; + bool no_more_finds = false; + const Glonass_Gnav_Ephemeris eph_glo = Glonass_Gnav_Ephemeris(); + const Gps_Ephemeris eph_gps = Gps_Ephemeris(); + + std::shared_ptr rp; + rp = std::make_shared(); + rp->rinex_obs_header(rp->obsFile, eph_gps, eph_glo, 0.0, "1C"); + + std::map gnss_pseudoranges_map; + + Gnss_Synchro gs1 = Gnss_Synchro(); + Gnss_Synchro gs2 = Gnss_Synchro(); + Gnss_Synchro gs3 = Gnss_Synchro(); + Gnss_Synchro gs4 = Gnss_Synchro(); + Gnss_Synchro gs5 = Gnss_Synchro(); + Gnss_Synchro gs6 = Gnss_Synchro(); + Gnss_Synchro gs7 = Gnss_Synchro(); + Gnss_Synchro gs8 = Gnss_Synchro(); + + std::string sys = "G"; + gs1.System = *sys.c_str(); + gs2.System = *sys.c_str(); + gs3.System = *sys.c_str(); + gs4.System = *sys.c_str(); + + sys = "R"; + gs5.System = *sys.c_str(); + gs6.System = *sys.c_str(); + gs7.System = *sys.c_str(); + gs8.System = *sys.c_str(); + + std::string sig = "1C"; + std::memcpy((void*)gs1.Signal, sig.c_str(), 3); + std::memcpy((void*)gs2.Signal, sig.c_str(), 3); + std::memcpy((void*)gs3.Signal, sig.c_str(), 3); + std::memcpy((void*)gs4.Signal, sig.c_str(), 3); + + sig = "1C"; + std::memcpy((void*)gs5.Signal, sig.c_str(), 3); + std::memcpy((void*)gs6.Signal, sig.c_str(), 3); + std::memcpy((void*)gs7.Signal, sig.c_str(), 3); + std::memcpy((void*)gs8.Signal, sig.c_str(), 3); + + gs1.PRN = 3; + gs2.PRN = 8; + gs3.PRN = 14; + gs4.PRN = 16; + gs5.PRN = 3; + gs6.PRN = 16; + gs7.PRN = 14; + gs8.PRN = 16; + + gs2.Pseudorange_m = 22000002.1; + gs2.Carrier_phase_rads = 45.4; + gs2.Carrier_Doppler_hz = 321; + gs2.CN0_dB_hz = 39; + + gs4.Pseudorange_m = 22000000; + gs4.Carrier_phase_rads = 23.4; + gs4.Carrier_Doppler_hz = -1534; + gs4.CN0_dB_hz = 40; + + gs6.Pseudorange_m = 22000000; + gs6.Carrier_phase_rads = 52.1; + gs6.Carrier_Doppler_hz = 1534; + gs6.CN0_dB_hz = 41; + + gs8.Pseudorange_m = 22000000; + gs8.Carrier_phase_rads = 0.8; + gs8.Carrier_Doppler_hz = -20; + gs8.CN0_dB_hz = 42; + + gnss_pseudoranges_map.insert( std::pair(1,gs1) ); + gnss_pseudoranges_map.insert( std::pair(2,gs2) ); + gnss_pseudoranges_map.insert( std::pair(3,gs3) ); + gnss_pseudoranges_map.insert( std::pair(4,gs4) ); + gnss_pseudoranges_map.insert( std::pair(5,gs5) ); + gnss_pseudoranges_map.insert( std::pair(6,gs6) ); + gnss_pseudoranges_map.insert( std::pair(7,gs7) ); + gnss_pseudoranges_map.insert( std::pair(8,gs8) ); + + rp->log_rinex_obs(rp->obsFile, eph_gps, eph_glo, 0.0, gnss_pseudoranges_map); + + rp->obsFile.seekp(0); + + while(!rp->obsFile.eof()) + { + std::getline(rp->obsFile, line_str); + if(!no_more_finds) + { + if (line_str.find("R16", 0) != std::string::npos) + { + no_more_finds = true; + line_aux = std::string(line_str); + } + } + } + std::string expected_str("R16 22000000.000 7 0.127 7 -20.000 7 42.000 22000000.000 6 8.292 6 1534.000 6 41.000"); + EXPECT_EQ(0, expected_str.compare(line_aux)); + + if(remove(rp->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; +} diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc index 11bd5d7cc..2f8307ee4 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc @@ -276,6 +276,33 @@ TEST(RtcmTest, MT1019) } +TEST(RtcmTest, MT1020) +{ + auto rtcm = std::make_shared(); + bool expected_true = true; + + Glonass_Gnav_Ephemeris glonass_gnav_eph = Glonass_Gnav_Ephemeris(); + Glonass_Gnav_Utc_Model glonass_gnav_utc_model = Glonass_Gnav_Utc_Model(); + Glonass_Gnav_Ephemeris glonass_gnav_eph_read = Glonass_Gnav_Ephemeris(); + Glonass_Gnav_Utc_Model glonass_gnav_utc_model_read = Glonass_Gnav_Utc_Model(); + + glonass_gnav_eph.i_satellite_PRN = 3; + glonass_gnav_eph.d_t_b = 4; + glonass_gnav_eph.d_E_n = 2.0 * E_LSB; + glonass_gnav_eph.d_l3rd_n = true; + glonass_gnav_utc_model.d_tau_gps = 5; + std::string tx_msg = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); + + EXPECT_EQ(0, rtcm->read_MT1020(tx_msg, glonass_gnav_eph_read, glonass_gnav_utc_model_read)); + EXPECT_EQ(3, glonass_gnav_eph_read.i_satellite_PRN); + EXPECT_DOUBLE_EQ(4, glonass_gnav_eph_read.d_t_b); + EXPECT_DOUBLE_EQ( 2.0 * E_LSB, glonass_gnav_eph_read.d_E_n); + EXPECT_DOUBLE_EQ( 5, glonass_gnav_utc_model_read.d_tau_gps); + EXPECT_EQ(expected_true, glonass_gnav_eph_read.d_l3rd_n); + EXPECT_EQ(1, rtcm->read_MT1020(rtcm->bin_to_binary_data(rtcm->hex_to_bin("FFFFFFFFFFF")), glonass_gnav_eph_read, glonass_gnav_utc_model_read)); +} + + TEST(RtcmTest, MT1029) { auto rtcm = std::make_shared(); @@ -321,6 +348,7 @@ TEST(RtcmTest, MSMCell) auto rtcm = std::make_shared(); Gps_Ephemeris gps_eph = Gps_Ephemeris(); Galileo_Ephemeris gal_eph = Galileo_Ephemeris(); + Glonass_Gnav_Ephemeris glo_gnav_eph = Glonass_Gnav_Ephemeris(); std::map pseudoranges; Gnss_Synchro gnss_synchro; @@ -328,15 +356,18 @@ TEST(RtcmTest, MSMCell) Gnss_Synchro gnss_synchro3; Gnss_Synchro gnss_synchro4; Gnss_Synchro gnss_synchro5; + Gnss_Synchro gnss_synchro6; gnss_synchro.PRN = 4; gnss_synchro2.PRN = 8; gnss_synchro3.PRN = 32; gnss_synchro4.PRN = 10; gnss_synchro5.PRN = 10; + gnss_synchro6.PRN = 10; std::string gps = "G"; std::string gal = "E"; + std::string glo = "R"; std::string c1 = "1C"; std::string s2 = "2S"; @@ -347,24 +378,28 @@ TEST(RtcmTest, MSMCell) gnss_synchro3.System = *gps.c_str(); gnss_synchro4.System = *gal.c_str(); gnss_synchro5.System = *gps.c_str(); + gnss_synchro6.System = *glo.c_str(); std::memcpy(static_cast(gnss_synchro.Signal), x5.c_str(), 3); std::memcpy(static_cast(gnss_synchro2.Signal), s2.c_str(), 3); std::memcpy(static_cast(gnss_synchro3.Signal), c1.c_str(), 3); std::memcpy(static_cast(gnss_synchro4.Signal), x5.c_str(), 3); std::memcpy(static_cast(gnss_synchro5.Signal), c1.c_str(), 3); + std::memcpy(static_cast(gnss_synchro6.Signal), c1.c_str(), 3); gnss_synchro.Pseudorange_m = 20000000.0; gnss_synchro2.Pseudorange_m = 20001010.0; gnss_synchro3.Pseudorange_m = 24002020.0; gnss_synchro4.Pseudorange_m = 20003010.1; gnss_synchro5.Pseudorange_m = 22003010.1; + gnss_synchro6.Pseudorange_m = 22003010.1; pseudoranges.insert(std::pair(1, gnss_synchro)); pseudoranges.insert(std::pair(2, gnss_synchro2)); pseudoranges.insert(std::pair(3, gnss_synchro3)); pseudoranges.insert(std::pair(4, gnss_synchro4)); pseudoranges.insert(std::pair(5, gnss_synchro5)); + pseudoranges.insert(std::pair(6, gnss_synchro5)); unsigned int ref_id = 1234; unsigned int clock_steering_indicator = 0; @@ -376,10 +411,12 @@ TEST(RtcmTest, MSMCell) gps_eph.i_satellite_PRN = gnss_synchro2.PRN; gal_eph.i_satellite_PRN = gnss_synchro.PRN; + glo_gnav_eph.i_satellite_PRN = gnss_synchro.PRN; std::string MSM1 = rtcm->print_MSM_1(gps_eph, {}, gal_eph, + glo_gnav_eph, obs_time, pseudoranges, ref_id, @@ -397,14 +434,17 @@ TEST(RtcmTest, MSMCell) EXPECT_EQ(0, MSM1_bin.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("001010101100")); // check cell mask std::map pseudoranges2; + pseudoranges2.insert(std::pair(1, gnss_synchro6)); pseudoranges2.insert(std::pair(1, gnss_synchro5)); pseudoranges2.insert(std::pair(2, gnss_synchro4)); pseudoranges2.insert(std::pair(3, gnss_synchro3)); pseudoranges2.insert(std::pair(4, gnss_synchro2)); pseudoranges2.insert(std::pair(5, gnss_synchro)); + pseudoranges2.insert(std::pair(6, gnss_synchro)); std::string MSM1_2 = rtcm->print_MSM_1(gps_eph, {}, gal_eph, + glo_gnav_eph, obs_time, pseudoranges2, ref_id, @@ -416,22 +456,23 @@ TEST(RtcmTest, MSMCell) std::string MSM1_bin_2 = rtcm->binary_data_to_bin(MSM1_2); EXPECT_EQ(0, MSM1_bin_2.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("001010101100")); // check cell mask - Gnss_Synchro gnss_synchro6; - gnss_synchro6.PRN = 10; - gnss_synchro6.System = *gps.c_str(); - std::memcpy(static_cast(gnss_synchro6.Signal), s2.c_str(), 3); - gnss_synchro6.Pseudorange_m = 24000000.0; + Gnss_Synchro gnss_synchro7; + gnss_synchro7.PRN = 10; + gnss_synchro7.System = *gps.c_str(); + std::memcpy(static_cast(gnss_synchro7.Signal), s2.c_str(), 3); + gnss_synchro7.Pseudorange_m = 24000000.0; std::map pseudoranges3; pseudoranges3.insert(std::pair(1, gnss_synchro)); pseudoranges3.insert(std::pair(2, gnss_synchro2)); - pseudoranges3.insert(std::pair(3, gnss_synchro6)); + pseudoranges3.insert(std::pair(3, gnss_synchro7)); pseudoranges3.insert(std::pair(4, gnss_synchro4)); pseudoranges3.insert(std::pair(5, gnss_synchro5)); std::string MSM1_3 = rtcm->print_MSM_1(gps_eph, {}, gal_eph, + {}, obs_time, pseudoranges3, ref_id, @@ -498,7 +539,7 @@ TEST(RtcmTest, MSM1) gps_eph.i_satellite_PRN = gnss_synchro.PRN; std::string MSM1 = rtcm->print_MSM_1(gps_eph, - {}, {}, + {}, {}, {}, obs_time, pseudoranges, ref_id, @@ -545,7 +586,7 @@ TEST(RtcmTest, MSM1) pseudoranges2.insert(std::pair(3, gnss_synchro2)); pseudoranges2.insert(std::pair(4, gnss_synchro)); std::string MSM1_2 = rtcm->print_MSM_1(gps_eph, - {}, {}, + {}, {}, {}, obs_time, pseudoranges2, ref_id, From 3f87223f350b4ca7e16548c2e713342d029d06f1 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 16 Aug 2017 21:40:05 -0700 Subject: [PATCH 33/48] bugfix: Fix compile errors, code style and documentation Fixes compilation errors after the RTCM package addition in the PVT block. It also adds a series of class members documentation in the code using the Doxygen standard while fixing code style errors --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 7 + .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 186 +++++++++++++++++- src/algorithms/PVT/libs/rtklib_solver.h | 4 +- .../libs/rtklib/rtklib_conversions.cc | 2 +- .../system_parameters/glonass_gnav_almanac.cc | 16 +- .../system_parameters/glonass_gnav_almanac.h | 24 ++- .../glonass_gnav_ephemeris.cc | 6 +- .../glonass_gnav_ephemeris.h | 13 +- .../glonass_gnav_navigation_message.cc | 50 ++++- .../glonass_gnav_navigation_message.h | 5 +- .../glonass_gnav_utc_model.cc | 4 +- .../glonass_gnav_utc_model.h | 6 +- src/core/system_parameters/rtcm.cc | 16 +- src/core/system_parameters/rtcm.h | 40 ++-- .../glonass_gnav_almanac_test.cc | 5 +- 15 files changed, 311 insertions(+), 73 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 9a4c0b899..757a2f9d0 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -84,17 +84,24 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); // RTCM message rates: least common multiple with output_rate_ms int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms); + int rtcm_MT1020_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1020_rate_ms", 5000), output_rate_ms); int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms); int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); + int rtcm_MT1087_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1087_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); std::map rtcm_msg_rate_ms; rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; + rtcm_msg_rate_ms[1020] = rtcm_MT1020_rate_ms; rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; for (int k = 1071; k < 1078; k++) // All GPS MSM { rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; } + for (int k = 1081; k < 1088; k++) // All GLONASS MSM + { + rtcm_msg_rate_ms[k] = rtcm_MT1087_rate_ms; + } for (int k = 1091; k < 1098; k++) // All Galileo MSM { rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index cbc13813a..1ebf824f7 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -180,6 +180,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) d_ls_pvt->galileo_almanac = *galileo_almanac; DLOG(INFO) << "New Galileo Almanac has arrived "; } + //**************** GLONASS GNAV Telemetry ************************** else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) { @@ -207,7 +208,6 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) // ### GLONASS GNAV Almanac ### std::shared_ptr glonass_gnav_almanac; glonass_gnav_almanac = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map d_ls_pvt->glonass_gnav_almanac = *glonass_gnav_almanac; DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "; } @@ -279,6 +279,14 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump { d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set } + if(rtcm_msg_rate_ms.find(1020) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1020_rate_ms = rtcm_msg_rate_ms[1020]; + } + else + { + d_rtcm_MT1020_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set + } if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) { d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; @@ -295,6 +303,14 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump { d_rtcm_MT1077_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set } + if(rtcm_msg_rate_ms.find(1087) != rtcm_msg_rate_ms.end()) // whatever between 1081 and 1087 + { + d_rtcm_MT1087_rate_ms = rtcm_msg_rate_ms[1087]; + } + else + { + d_rtcm_MT1087_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + } if(rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 { d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; @@ -316,8 +332,10 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump d_rx_time = 0.0; last_pvt_display_T_rx_s = 0.0; last_RTCM_1019_output_time = 0.0; + last_RTCM_1020_output_time = 0.0; last_RTCM_1045_output_time = 0.0; last_RTCM_1077_output_time = 0.0; + last_RTCM_1087_output_time = 0.0; last_RTCM_1097_output_time = 0.0; last_RTCM_MSM_output_time = 0.0; last_RINEX_obs_output_time = 0.0; @@ -499,8 +517,10 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite bool flag_display_pvt = false; bool flag_compute_pvt_output = false; bool flag_write_RTCM_1019_output = false; + bool flag_write_RTCM_1020_output = false; bool flag_write_RTCM_1045_output = false; bool flag_write_RTCM_1077_output = false; + bool flag_write_RTCM_1087_output = false; bool flag_write_RTCM_1097_output = false; bool flag_write_RTCM_MSM_output = false; bool flag_write_RINEX_obs_output = false; @@ -593,7 +613,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite flag_write_RTCM_1019_output = true; last_RTCM_1019_output_time = current_RX_time; } - + if ((std::fabs(current_RX_time - last_RTCM_1020_output_time) * 1000.0 >= static_cast(d_rtcm_MT1020_rate_ms)) && (d_rtcm_MT1020_rate_ms != 0) ) // allows deactivating messages by setting rate = 0 + { + flag_write_RTCM_1020_output = true; + last_RTCM_1020_output_time = current_RX_time; + } if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast(d_rtcm_MT1045_rate_ms)) && (d_rtcm_MT1045_rate_ms != 0) ) { flag_write_RTCM_1045_output = true; @@ -605,7 +629,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite flag_write_RTCM_1077_output = true; last_RTCM_1077_output_time = current_RX_time; } - + if ((std::fabs(current_RX_time - last_RTCM_1087_output_time) * 1000.0 >= static_cast(d_rtcm_MT1087_rate_ms)) && (d_rtcm_MT1087_rate_ms != 0) ) + { + flag_write_RTCM_1087_output = true; + last_RTCM_1087_output_time = current_RX_time; + } if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast(d_rtcm_MT1097_rate_ms)) && (d_rtcm_MT1097_rate_ms != 0) ) { flag_write_RTCM_1097_output = true; @@ -1097,7 +1125,6 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } // ####################### RTCM MESSAGES ################# - //TODO Add RTCM logic for GLONASS satellites if(b_rtcm_writing_started) { if(type_of_rx == 1) // GPS L1 C/A @@ -1226,6 +1253,157 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } } } + if((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + { + if(flag_write_RTCM_1020_output == true) + { + for(std::map::iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end(); glonass_gnav_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + + std::map::iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); + + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if(type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A + { + if(flag_write_RTCM_1019_output == true) + { + for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(flag_write_RTCM_1020_output == true) + { + for(std::map::iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end(); glonass_gnav_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if(flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(glo_channel == 0) + { + if(system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = i; + } + } + } + i++; + } + if(flag_write_RTCM_MSM_output == true) + { + + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if(flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + if(type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B + { + if(flag_write_RTCM_1020_output == true) + { + for(std::map::iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end(); glonass_gnav_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if(flag_write_RTCM_1045_output == true) + { + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if(flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(glo_channel == 0) + { + if(system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = i; + } + } + } + i++; + } + if(flag_write_RTCM_MSM_output == true) + { + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if(flag_write_RTCM_MSM_output == true) + { + + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } } if(!b_rtcm_writing_started) // the first time diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 274289138..5e74aa357 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -99,8 +99,8 @@ public: Gps_CNAV_Iono gps_cnav_iono; Gps_CNAV_Utc_Model gps_cnav_utc_model; - Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< GLONASS GNAV UTC Model - Glonass_Gnav_Almanac glonass_gnav_almanac; //!< GLONASS GNAV Almanac Model + Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< Map storing GLONASS GNAV UTC Model + Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model int count_valid_position; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index f4ffafae8..45f13db82 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -74,7 +74,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number; /* satellite number */ rtklib_sat.iode = glonass_gnav_eph.d_t_b; /* IODE (0-6 bit of tb field) */ rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */ - rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ + rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ rtklib_sat.sva = glonass_gnav_eph.d_F_T; /* satellite accuracy*/ rtklib_sat.age = glonass_gnav_eph.d_E_n; /* satellite age*/ rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn*1000; /* satellite position (ecef) (m) */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index 002324e94..cbef6df64 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -1,8 +1,9 @@ /*! * \file glonass_gnav_almanac.cc * \brief Interface of a GLONASS GNAV ALMANAC storage as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf - * \author Damian Miralles , 2017. dmiralles2009(at)gmail.com + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * @@ -36,10 +37,9 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() { i_satellite_freq_channel = 0; - d_tau_c = 0.0; - d_tau_gps = 0.0; - d_N_4 = 0.0; - d_N_A = 0.0; + i_satellite_PRN = 0; + i_satellite_slot_number = 0; + d_n_A = 0.0; d_H_n_A = 0.0; d_lambda_n_A = 0.0; @@ -56,7 +56,7 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() d_l_n = 0.0; } -void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) +void Glonass_Gnav_Almanac::satellite_position(double N_A, double N_i, double t_i) { double T_nom = 43200; // [seconds] double i_nom = D2R*63.0; // [rad] @@ -85,7 +85,7 @@ void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) double e2_y = 0.0; double e2_z = 0.0; // Compute time difference to reference time - Delta_t = (N_i - d_N_A) * 86400 + (t_i + d_t_lambda_n_A); + Delta_t = (N_i - N_A) * 86400 + (t_i + d_t_lambda_n_A); // Compute the actual inclination i = i_nom + d_Delta_i_n_A; diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 1e1682e66..aafacb888 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -39,19 +39,14 @@ /*! * \brief This class is a storage for the GLONASS SV ALMANAC data as described GLONASS ICD (Edition 5.1) - * - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Almanac { public: - int i_satellite_freq_channel; //!< SV Frequency Channel NUMBER - double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] - double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] - double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] - double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days] double d_n_A; //!< Conventional number of satellite within GLONASS space segment [dimensionless] - double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] + double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite as table 4.10 (0-31) [dimensionless] double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] double d_t_lambda_n_A; //!< Time of first ascending node passage [s] double d_Delta_i_n_A; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] @@ -65,6 +60,11 @@ public: double d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + // Satellite Identification Information + int i_satellite_freq_channel; //!< SV Frequency Channel Number + unsigned int i_satellite_PRN; //!< SV PRN Number, equivalent to slot number for compatibility with GPS + unsigned int i_satellite_slot_number; //!< SV Slot Number + // satellite positions double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. double d_satpos_Yo; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] @@ -93,10 +93,8 @@ public: if(version){}; archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel); - archive & make_nvp("d_tau_c", d_tau_c); - archive & make_nvp("d_tau_gps", d_tau_gps); - archive & make_nvp("d_N_4", d_N_4); - archive & make_nvp("d_N_A", d_N_A); + archive & make_nvp("i_satellite_PRN", i_satellite_PRN); + archive & make_nvp("i_satellite_slot_number", i_satellite_slot_number); archive & make_nvp("d_n_A", d_n_A); archive & make_nvp("d_H_n_A", d_H_n_A); archive & make_nvp("d_lambda_n_A", d_lambda_n_A); @@ -113,7 +111,7 @@ public: archive & make_nvp("d_l_n", d_l_n); } - void satellite_position(double N_i, double t_i); + void satellite_position(double N_A, double N_i, double t_i); /*! * Default constructor */ diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index ba376b620..f4d47ae53 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -1,11 +1,9 @@ /*! * \file glonass_gnav_ephemeris.cc * \brief Interface of a GLONASS GNAV EPHEMERIS storage and orbital model functions - * - * See Russian Institute of Space Device Engineering, - “Global Navigation Satellite System GLONASS Interface Control Document, - Navigational radiosignal in bands L1, L2,” Moscow, Edition 5.1, 2008. + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 3f8fa699c..447ced86d 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -43,8 +43,8 @@ /*! * \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD (Edition 5.1) - * - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Ephemeris { @@ -100,9 +100,10 @@ public: // Inmediate deliverables of ephemeris information //TODO check how freq channel is managed in gnav message. I think it is a number greater thn 0 - unsigned int i_satellite_freq_channel; //!< SV Frequency Channel Number - unsigned int i_satellite_PRN; //!< SV PRN NUMBER - unsigned int i_satellite_slot_number; //!< SV PRN NUMBER + // Satellite Identification Information + int i_satellite_freq_channel; //!< SV Frequency Channel Number + unsigned int i_satellite_PRN; //!< SV PRN Number, equivalent to slot number for compatibility with GPS + unsigned int i_satellite_slot_number; //!< SV Slot Number double d_TOD; //!< Time of Day of the ephemeris set based in start of frame [s] double d_D4Y; //!< Day of Year after latest leap year (4 year interval) double d_yr; //!< Current year @@ -139,6 +140,8 @@ public: if(version){}; archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel); //!< SV PRN frequency channel number + archive & make_nvp("i_satellite_PRN", i_satellite_PRN); + archive & make_nvp("i_satellite_slot_number", i_satellite_slot_number); archive & make_nvp("d_m", d_m); //!< String number within frame [dimensionless] archive & make_nvp("d_t_k", d_t_k); //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] archive & make_nvp("d_t_b", d_t_b); //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 7cdbec768..b234bad71 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -1,8 +1,9 @@ /*! -m * \file glonass_gnav_navigation_message.cc + * \file glonass_gnav_navigation_message.cc * \brief Implementation of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * @@ -416,6 +417,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // Fill in ephemeris deliverables in the code gnav_ephemeris.i_satellite_slot_number = gnav_ephemeris.d_n; + gnav_ephemeris.i_satellite_PRN = gnav_ephemeris.d_n; gnav_ephemeris.d_D4Y = gnav_ephemeris.d_N_T; flag_ephemeris_str_4 = true; @@ -480,12 +482,20 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + if(i_satellite_slot_number == gnav_ephemeris.i_satellite_slot_number) { - gnav_ephemeris.i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A; + gnav_ephemeris.i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel; } flag_almanac_str_7 = true; } @@ -521,6 +531,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) -32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_9 = true; } case 10: @@ -551,6 +569,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_11 = true; } break; @@ -582,6 +608,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_13 = true; } case 14: @@ -621,6 +655,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_15 = true; } default: diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 8fbb30e22..97e2a8d49 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -48,9 +48,8 @@ /*! * \brief This class decodes a GLONASS GNAV Data message as described in GLONASS ICD (Edition 5.1) - * - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdfdescribed in IS-GPS-200E - * + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Navigation_Message { diff --git a/src/core/system_parameters/glonass_gnav_utc_model.cc b/src/core/system_parameters/glonass_gnav_utc_model.cc index 55ca5ccee..ec9d5e20e 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.cc +++ b/src/core/system_parameters/glonass_gnav_utc_model.cc @@ -1,7 +1,9 @@ /* * \file glonass_gnav_utc_model.h * \brief Interface of a GLONASS GNAV UTC MODEL storage - * \author Damian Miralles, 2017. dmiralles2009(at).gmail.com + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index 6aca883f4..fa0886418 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -38,13 +38,14 @@ /*! * \brief This class is a storage for the GLONASS GNAV UTC MODEL data as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Utc_Model { public: bool valid; - // UTC + // Clock Parameters double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] @@ -60,7 +61,6 @@ public: { using boost::serialization::make_nvp; if(version){}; - archive & make_nvp("valid",valid); archive & make_nvp("d_tau_c", d_tau_c); archive & make_nvp("d_tau_gps", d_tau_gps); diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 3a82d5647..662006d92 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -1848,7 +1848,7 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.i_satellite_slot_number = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 6))); index += 6; - glonass_gnav_eph.i_satellite_freq_channel = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + glonass_gnav_eph.i_satellite_freq_channel = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)) - 7.0); index += 5; glonass_gnav_alm_health = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); @@ -3882,12 +3882,14 @@ int Rtcm::set_DF039(bool code_indicator) } -int Rtcm::set_DF040(unsigned int frequency_channel_number) +int Rtcm::set_DF040(int frequency_channel_number) { - unsigned int freq_ = frequency_channel_number; + unsigned int freq_ = frequency_channel_number + 7; if(freq_ > 20) { - LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; + LOG(WARNING) << "GLONASS Satellite Frequency Number Conversion Error." + << "Value must be between 0 and 20, but converted channel" + << "frequency number " << freq_ << " was found"; } DF040 = std::bitset<5>(freq_); @@ -3897,10 +3899,12 @@ int Rtcm::set_DF040(unsigned int frequency_channel_number) int Rtcm::set_DF040(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - unsigned int freq_ = glonass_gnav_eph.i_satellite_freq_channel; + unsigned int freq_ = glonass_gnav_eph.i_satellite_freq_channel + 7; if(freq_ > 20) { - LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; + LOG(WARNING) << "GLONASS Satellite Frequency Number Conversion Error." + << "Value must be between 0 and 20, but converted channel" + << "frequency number " << freq_ << " was found"; } DF040 = std::bitset<5>(freq_); diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index ed4369923..3ba745217 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -133,9 +133,9 @@ public: * \brief Prints L1-Only GLONASS RTK Observables * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id); @@ -143,9 +143,9 @@ public: * \brief Prints Extended L1-Only GLONASS RTK Observables * \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id); @@ -153,9 +153,9 @@ public: * \brief Prints L1&L2 GLONASS RTK Observables * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); @@ -163,9 +163,9 @@ public: * \brief Prints Extended L1&L2 GLONASS RTK Observables * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); @@ -333,9 +333,9 @@ public: /*! * \brief Locks time period in which GLONASS signals have been continually tracked. * \note Code added as part of GSoC 2017 program - * \params eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return Returns the time period in which GLONASS signals have been continually tracked. */ unsigned int lock_time(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); @@ -1015,8 +1015,14 @@ private: std::bitset<8> DF032; - std::bitset<27> DF034; //!< GLONASS Epoch Time (tk) + /*! + * \brief Sets the Data Field value + * \note Code added as part of GSoC 2017 program + * \param obs_time Time of observation at the moment of printing + * \return returns 0 upon success + */ int set_DF034(double obs_time); + std::bitset<27> DF034; //!< GLONASS Epoch Time (tk) std::bitset<5> DF035; //!< No. of GLONASS Satellite Signals Processed int set_DF035(const std::map & observables); @@ -1035,7 +1041,7 @@ private: int set_DF039(bool code_indicator); std::bitset<5> DF040; //!< GLONASS Satellite Frequency Number - int set_DF040(unsigned int frequency_channel_number); + int set_DF040(int frequency_channel_number); int set_DF040(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); std::bitset<25> DF041; //!< GLONASS L1 Pseudorange diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc index 6c02311cf..89cf2ae1d 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc @@ -53,10 +53,11 @@ TEST(GlonassGnavAlmanacTest, SatellitePosition) double Vxoi = -3.375497; // [m/s] double Vyoi = -0.161453; // [Кm/s] double Vzoi = 2.060844; // [Кm/s] + double N_A = 615; // [days] Glonass_Gnav_Almanac gnav_almanac; - gnav_almanac.d_N_A = 615; // [days] + gnav_almanac.d_lambda_n_A = -0.189986229; // [half cycles] gnav_almanac.d_t_lambda_n_A = 27122.09375; // [second] gnav_almanac.d_Delta_i_n_A = 0.011929512; // [half cycle] @@ -65,7 +66,7 @@ TEST(GlonassGnavAlmanacTest, SatellitePosition) gnav_almanac.d_epsilon_n_A = 0.001482010; // [unitless] gnav_almanac.d_omega_n_A = 0.440277100; // [Half cycle] - gnav_almanac.satellite_position(N_i, t_i); + gnav_almanac.satellite_position(N_A, N_i, t_i); ASSERT_TRUE(gnav_almanac.d_satpos_Xo - Xoi < DBL_EPSILON ); ASSERT_TRUE(gnav_almanac.d_satpos_Yo - Yoi < DBL_EPSILON ); From eb33715cb94e99607a47f489823feb6006574f33 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sat, 19 Aug 2017 14:04:14 -0700 Subject: [PATCH 34/48] unit-test: Adds and fixes unit test for system-parameters block Adding unit test for system-parameters block, testing string decoding logic for GLONASS GNAV messages. Bug fixes the code after several errors were detected while debugging --- src/algorithms/PVT/libs/rinex_printer.cc | 18 +- src/core/system_parameters/GLONASS_L1_CA.h | 4 +- .../system_parameters/glonass_gnav_almanac.h | 4 +- .../glonass_gnav_ephemeris.cc | 71 +++++--- .../glonass_gnav_ephemeris.h | 7 +- .../glonass_gnav_navigation_message.cc | 119 +++++-------- .../glonass_gnav_navigation_message.h | 3 +- src/tests/test_main.cc | 1 + .../signal-processing-blocks/pvt/rtcm_test.cc | 51 ++++-- .../gps_l1_ca_telemetry_decoder_test.cc | 3 +- .../glonass_gnav_almanac_test.cc | 7 +- .../glonass_gnav_ephemeris_test.cc | 37 ++-- .../glonass_gnav_nav_message_test.cc | 161 ++++++++++++++---- 13 files changed, 311 insertions(+), 175 deletions(-) diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 362b6f289..facaa6f5e 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -3293,14 +3293,13 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 3); line += std::string(1, ' '); - // TODO Add this here, we need to know all satellites in system to get this done - line += satelliteSystem["GLONASS"]; - line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 2); // Slot Number - line += std::string(1, ' '); - line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 2); // Frequency Number - line += std::string(1, ' '); - - + // TODO Add this here, we need to know all satellites in system to get this done + line += satelliteSystem["GLONASS"]; + line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 2); // Slot Number + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(boost::lexical_cast(0), 2); // Frequency Number + line += std::string(1, ' '); + line += std::string(60-line.size(), ' '); line += Rinex_Printer::leftJustify("GLONASS SLOT / FRQ #", 20); Rinex_Printer::lengthCheck(line); out << line << std::endl; @@ -3328,7 +3327,8 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem line += observationCode["GLONASS_G2_P"]; line += std::string(1, ' '); line += Rinex_Printer::rightJustify(asString(0.0, 3), 8); - line += Rinex_Printer::leftJustify("GLONASS SLOT / FRQ #", 20); + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("GLONASS COD/PHS/BIS", 20); Rinex_Printer::lengthCheck(line); out << line << std::endl; diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 7532ceb3a..3a93d0def 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -127,8 +127,8 @@ const std::vector> KX({{78,8}}); //STRING 1 const std::vector> P1({{8,2}}); const std::vector> T_K_HR({{10,5}}); -const std::vector> T_K_MIN({{10,6}}); -const std::vector> T_K_SEC({{10,1}}); +const std::vector> T_K_MIN({{15,6}}); +const std::vector> T_K_SEC({{21,1}}); const std::vector> X_N_DOT ({{22,24}}); const std::vector> X_N_DOT_DOT ({{46,5}}); const std::vector> X_N({{51,27}}); diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index aafacb888..2c7780bf5 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -1,7 +1,9 @@ /*! * \file glonass_gnav_almanac.h * \brief Interface of a GLONASS GNAV ALMANAC storage - * \author Damian Miralles, 2017. dmiralles2009@gmail.com + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index f4d47ae53..3b66f7a12 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -39,13 +39,48 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() { - i_satellite_freq_channel = 0; - d_m = 0.0; //!< String number within frame [dimensionless] - d_t_k = 0.0; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] - d_t_b = 0.0; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] - d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless] - d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] - d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + d_m = 0.0; //!< String number within frame [dimensionless] + d_t_k = 0.0; //!< GLONASS Time (UTC(SU) + 3 h) referenced to the beginning of the frame within the current day [s] + d_t_b = 0.0; //!< 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] + d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless] + d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] + d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), + d_Xn = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. + d_Yn = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] + d_Zn = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] + d_VXn = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] + d_VYn = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] + d_VZn = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] + d_AXn = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] + d_AYn = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] + d_AZn = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] + d_B_n = 0.0; //!< Health flag [dimensionless] + d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] + d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] + d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] + d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation + d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] + d_E_n = 0.0; //!< Characterises "age" of a current information [days] + d_P_1 = 0.0; //!< Flag of the immediate data updating [minutes] + d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + d_l3rd_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + d_l5th_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + + // Satellite Identification Information + i_satellite_freq_channel = 0; //!< SV Frequency Channel Number + i_satellite_PRN = 0; //!< SV PRN Number, equivalent to slot number for compatibility with GPS + i_satellite_slot_number = 0; //!< SV Slot Number + d_TOD = 0.0; //!< Time of Day of the ephemeris set based in start of frame [s] + d_D4Y = 0.0; //!< Day of Year after latest leap year (4 year interval) + d_yr = 1972; //!< Current year, defaults to 1972 (UTC Epoch with leap seconds) + d_satClkDrift = 0.0; //!< GLONASS clock error + d_dtr = 0.0; //!< relativistic clock correction term + d_iode = 0.0; //!< Issue of data, ephemeris (Bit 0-6 of tb) + d_tau_c = 0.0; + d_TOW = 0.0; // tow of the start of frame + d_WN = 0.0; // week number of the start of frame // satellite positions d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] @@ -58,31 +93,15 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] - d_B_n = 0.0; //!< Health flag [dimensionless] - d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] - d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] - d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless] - d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation - d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] - d_E_n = 0.0; //!< Characterises "age" of a current information [days] - d_P_1 = 0.0; //!< Flag of the immediate data updating. - d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] - d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] - d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - d_l3rd_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - d_l5th_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - - // clock terms derived from ephemeris data - d_satClkDrift = 0.0; //!< GLONASS clock error - d_dtr = 0.0; } boost::posix_time::ptime Glonass_Gnav_Ephemeris::compute_GLONASS_time(const double offset_time) const { boost::posix_time::time_duration t(0, 0, offset_time); - boost::gregorian::date d(d_yr, 1, d_N_T); - boost::posix_time::ptime glonass_time(d, t); + boost::gregorian::date d1(d_yr, 1, 1); + boost::gregorian::days d2(d_N_T); + boost::posix_time::ptime glonass_time(d1+d2, t); return glonass_time; } diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 447ced86d..8a978f816 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -1,7 +1,9 @@ /*! * \file glonass_gnav_ephemeris.h * \brief Interface of a GLONASS EPHEMERIS storage + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * @@ -99,9 +101,8 @@ public: double 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] // Inmediate deliverables of ephemeris information - //TODO check how freq channel is managed in gnav message. I think it is a number greater thn 0 // Satellite Identification Information - int i_satellite_freq_channel; //!< SV Frequency Channel Number + int i_satellite_freq_channel; //!< SV Frequency Channel Number unsigned int i_satellite_PRN; //!< SV PRN Number, equivalent to slot number for compatibility with GPS unsigned int i_satellite_slot_number; //!< SV Slot Number double d_TOD; //!< Time of Day of the ephemeris set based in start of frame [s] @@ -114,8 +115,6 @@ public: double d_TOW; // tow of the start of frame double d_WN; // week number of the start of frame - // Need to add a way to compute the GPS week number and GPS TIME OF WEEK from GLONASS ephemeris - // satellite positions after RK4 Integration double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index b234bad71..916b3004a 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -31,6 +31,8 @@ */ #include "glonass_gnav_navigation_message.h" +#include +#include #include #include #include @@ -107,8 +109,8 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message() bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset bits) { - int sum_bits; - int sum_hamming; + int sum_bits = 0; + int sum_hamming = 0; int C1 = 0; int C2 = 0; int C3 = 0; @@ -117,85 +119,84 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset string_bits(GLONASS_GNAV_STRING_BITS); - std::bitset data = std::bitset(bits.to_string(), 0, 77); - std::bitset hamming_code = std::bitset(bits.to_string(), 77, 8); + //!< Populate data and hamming code vectors + for(int i = 0; i < static_cast(GLONASS_GNAV_STRING_BITS); i++) + { + string_bits[i] = static_cast(bits[i]); + } - std::istringstream dsb = std::istringstream( data.to_string() ); - std::istringstream hcb = std::istringstream( hamming_code.to_string() ); - std::vector data_bits = std::vector( std::istream_iterator( dsb ), std::istream_iterator() ); - std::vector hamming_code_bits = std::vector( std::istream_iterator( dsb ), std::istream_iterator() ); //!< Compute C1 term sum_bits = 0; for(int i = 0; i < static_cast(GLONASS_GNAV_CRC_I_INDEX.size()); i++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_I_INDEX[i]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_I_INDEX[i]]; } - C1 = hamming_code_bits[0]^(sum_bits%2); + C1 = string_bits[0]^(sum_bits%2); //!< Compute C2 term sum_bits = 0; for(int j = 0; j < static_cast(GLONASS_GNAV_CRC_J_INDEX.size()); j++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_J_INDEX[j]]; } - C2 = (hamming_code_bits[1])^(sum_bits%2); + C2 = (string_bits[1])^(sum_bits%2); //!< Compute C3 term sum_bits = 0; for(int k = 0; k < static_cast(GLONASS_GNAV_CRC_K_INDEX.size()); k++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_K_INDEX[k]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_K_INDEX[k]]; } - C3 = hamming_code_bits[2]^(sum_bits%2); + C3 = string_bits[2]^(sum_bits%2); //!< Compute C4 term sum_bits = 0; for(int l = 0; l < static_cast(GLONASS_GNAV_CRC_L_INDEX.size()); l++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_L_INDEX[l]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_L_INDEX[l]]; } - C4 = hamming_code_bits[3]^(sum_bits%2); + C4 = string_bits[3]^(sum_bits%2); //!< Compute C5 term sum_bits = 0; for(int m = 0; m < static_cast(GLONASS_GNAV_CRC_M_INDEX.size()); m++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_M_INDEX[m]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_M_INDEX[m]]; } - C5 = hamming_code_bits[4]^(sum_bits%2); + C5 = string_bits[4]^(sum_bits%2); //!< Compute C6 term sum_bits = 0; for(int n = 0; n < static_cast(GLONASS_GNAV_CRC_N_INDEX.size()); n++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_N_INDEX[n]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_N_INDEX[n]]; } - C6 = hamming_code_bits[5]^(sum_bits%2); + C6 = string_bits[5]^(sum_bits%2); //!< Compute C7 term sum_bits = 0; for(int p = 0; p < static_cast(GLONASS_GNAV_CRC_P_INDEX.size()); p++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_P_INDEX[p]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_P_INDEX[p]]; } - C7 = hamming_code_bits[6]^(sum_bits%2); + C7 = string_bits[6]^(sum_bits%2); //!< Compute C_Sigma term sum_bits = 0; sum_hamming = 0; for(int q = 0; q < static_cast(GLONASS_GNAV_CRC_Q_INDEX.size()); q++) { - sum_bits += data_bits[GLONASS_GNAV_CRC_Q_INDEX[q]]; + sum_bits += string_bits[GLONASS_GNAV_CRC_Q_INDEX[q]]; } for(int q = 0; q < 8; q++) { - sum_hamming += hamming_code_bits[q]; + sum_hamming += string_bits[q]; } C_Sigma = (sum_hamming%2)^(sum_bits%2); - //!< Verification of the data // All of the checksums are equal to zero if((C1 & C2 & C3 & C4 & C5 & C6 & C7 & C_Sigma) == 0 ) @@ -252,60 +253,29 @@ unsigned long int Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std: signed long int Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) { signed long int value = 0; + signed long int sign = 0; int num_of_slices = parameter.size(); - // Discriminate between 64 bits and 32 bits compiler - int long_int_size_bytes = sizeof(signed long int); - if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system + // read the MSB and perform the sign extension + if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) + { + sign = -1; + } + else + { + sign = 1; + } + for (int i = 0; i < num_of_slices; i++) { - // read the MSB and perform the sign extension - if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) + for (int j = 1; j < parameter[i].second; j++) { - value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable - } - else - { - value &= 0; - } - - for (int i = 0; i < num_of_slices; i++) - { - for (int j = 0; j < parameter[i].second; j++) + value <<= 1; //shift left + if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) { - value <<= 1; //shift left - value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable) - if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) - { - value += 1; // insert the bit - } + value += 1; // insert the bit } } } - else // we assume we are in a 32 bits system - { - // read the MSB and perform the sign extension - if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) - { - value ^= 0xFFFFFFFF; - } - else - { - value &= 0; - } - - for (int i = 0; i < num_of_slices; i++) - { - for (int j = 0; j < parameter[i].second; j++) - { - value <<= 1; //shift left - value &= 0xFFFFFFFE; //reset the corresponding bit - if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) - { - value += 1; // insert the bit - } - } - } - } - return value; + return (sign*value); } @@ -393,7 +363,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) case 3: // --- It is string 3 ---------------------------------------------- gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); - gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * TWO_N30; + gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * TWO_N40; gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); gnav_ephemeris.d_l3rd_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)) * TWO_N20; @@ -541,6 +511,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) flag_almanac_str_9 = true; } + break; case 10: // --- It is string 10 --------------------------------------------- i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); @@ -618,6 +589,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) flag_almanac_str_13 = true; } + break; case 14: // --- It is string 14 --------------------------------------------- if( frame_ID == 5) @@ -665,6 +637,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) flag_almanac_str_15 = true; } + break; default: break; } // switch subframeID ... diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 97e2a8d49..88468e380 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -1,8 +1,9 @@ /*! * \file glonass_gnav_navigation_message.h * \brief Interface of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 2f4df5220..e27506c34 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -145,6 +145,7 @@ DECLARE_string(log_dir); #include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" #include "unit-tests/system-parameters/glonass_gnav_almanac_test.cc" +#include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc index 2f8307ee4..262a8fd6e 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc @@ -281,20 +281,49 @@ TEST(RtcmTest, MT1020) auto rtcm = std::make_shared(); bool expected_true = true; - Glonass_Gnav_Ephemeris glonass_gnav_eph = Glonass_Gnav_Ephemeris(); - Glonass_Gnav_Utc_Model glonass_gnav_utc_model = Glonass_Gnav_Utc_Model(); - Glonass_Gnav_Ephemeris glonass_gnav_eph_read = Glonass_Gnav_Ephemeris(); - Glonass_Gnav_Utc_Model glonass_gnav_utc_model_read = Glonass_Gnav_Utc_Model(); + // Objects to populate the ephemeris and utc fields + Glonass_Gnav_Ephemeris gnav_eph = Glonass_Gnav_Ephemeris(); + Glonass_Gnav_Utc_Model gnav_utc_model = Glonass_Gnav_Utc_Model(); + // Objects read, used for comparison + Glonass_Gnav_Ephemeris gnav_eph_read = Glonass_Gnav_Ephemeris(); + Glonass_Gnav_Utc_Model gnav_utc_model_read = Glonass_Gnav_Utc_Model(); - glonass_gnav_eph.i_satellite_PRN = 3; - glonass_gnav_eph.d_t_b = 4; - glonass_gnav_eph.d_E_n = 2.0 * E_LSB; - glonass_gnav_eph.d_l3rd_n = true; - glonass_gnav_utc_model.d_tau_gps = 5; - std::string tx_msg = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); + glonass_gnav_eph.i_satellite_slot_number = 3; + gnav_ephemeris.d_P_1 = 0; + gnav_ephemeris.d_t_k = 7560; + gnav_ephemeris.d_VXn = -0.490900039672852; + gnav_ephemeris.d_AXn = 0; + gnav_ephemeris.d_Xn = -11025.6669921875; + gnav_ephemeris.d_B_n = 0; + gnav_ephemeris.d_P_2 = 1; + gnav_ephemeris.d_t_b = 8100; + gnav_ephemeris.d_VYn = -2.69022750854492; + gnav_ephemeris.d_AYn = 0; + gnav_ephemeris.d_Yn = -11456.7348632812; + gnav_ephemeris.d_P_3 = 1; + gnav_ephemeris.d_gamma_n = 1.81898940354586e-12; + gnav_ephemeris.d_P = 3; + gnav_ephemeris.d_l3rd_n = 0; + gnav_ephemeris.d_VZn = -1.82016849517822; + gnav_ephemeris.d_AZn = -2.79396772384644e-09; + gnav_ephemeris.d_Zn = 19929.2377929688; + gnav_ephemeris.d_tau_n = -8.30907374620438e-05; + gnav_ephemeris.d_Delta_tau_n = 9.31322574615479e-10; + gnav_ephemeris.d_E_n = 0; + gnav_ephemeris.d_P_4 = 0; + gnav_ephemeris.d_F_T = 6; + gnav_ephemeris.d_N_T = 268; + gnav_ephemeris.d_n = 21; + gnav_ephemeris.d_M = 1; + gnav_utc_model.d_N_A = 268; + gnav_utc_model.d_tau_c = 9.6391886472702e-08; + gnav_utc_model.d_N_4 = 6; + gnav_utc_model.d_tau_gps = 9.313225746154785e-08; + + std::string tx_msg = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); EXPECT_EQ(0, rtcm->read_MT1020(tx_msg, glonass_gnav_eph_read, glonass_gnav_utc_model_read)); - EXPECT_EQ(3, glonass_gnav_eph_read.i_satellite_PRN); + EXPECT_EQ(3, glonass_gnav_eph_read.i_satellite_slot_number); EXPECT_DOUBLE_EQ(4, glonass_gnav_eph_read.d_t_b); EXPECT_DOUBLE_EQ( 2.0 * E_LSB, glonass_gnav_eph_read.d_E_n); EXPECT_DOUBLE_EQ( 5, glonass_gnav_utc_model_read.d_tau_gps); diff --git a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc index 91979878b..e049eb45d 100644 --- a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc @@ -1,6 +1,6 @@ /*! * \file gps_l1_ca_dll_pll_tracking_test.cc - * \brief This class implements a tracking test for Galileo_E5a_DLL_PLL_Tracking + * \brief This class implements a telemetry decoder test for GPS_L1_CA_Telemetry_Decoder * implementation based on some input parameters. * \author Javier Arribas, 2015. jarribas(at)cttc.es * @@ -477,4 +477,3 @@ TEST_F(GpsL1CATelemetryDecoderTest, ValidationOfResults) std::cout << "Test completed in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; } - diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc index 89cf2ae1d..2c36f03cb 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc @@ -1,7 +1,8 @@ /*! * \file code_generation_test.cc - * \brief This file implements tests for the generation of complex exponentials. - * \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * * ------------------------------------------------------------------------- @@ -35,8 +36,6 @@ #include "gps_sdr_signal_processing.h" #include "gnss_signal_processing.h" - - #include #include #include "gnss_signal_processing.h" diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc index 44497f114..b87cba16a 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc @@ -1,7 +1,8 @@ /*! * \file code_generation_test.cc - * \brief This file implements tests for the generation of complex exponentials. - * \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * * ------------------------------------------------------------------------- @@ -32,23 +33,33 @@ #include #include +#include #include "gnss_signal_processing.h" #include "glonass_gnav_ephemeris.h" -TEST(GlonassGnavEphemerisTest, SatellitePosition) +TEST(GlonassGnavEphemerisTest, ComputeGlonassTime) { Glonass_Gnav_Ephemeris gnav_eph; + gnav_eph.d_yr = 2016; + gnav_eph.d_N_T = 367; + boost::posix_time::time_duration t(0, 0, 7560); + boost::gregorian::date d(gnav_eph.d_yr, 1, 1); + boost::gregorian::days d2(gnav_eph.d_N_T); + d = d + d2; - gnav_eph.d_Xn = 12317.934082000; - gnav_eph.d_Yn = -2245.13232422; - gnav_eph.d_Zn = 22212.8173828; - gnav_eph.d_VXn = -1.25356674194; - gnav_eph.d_VYn = 2.774200439450; - gnav_eph.d_VZn = 0.9808206558230000; - gnav_eph.d_AXn = -0.931322574616e-9; - gnav_eph.d_AYn = 0.0000000000000000; - gnav_eph.d_AZn = -0.186264514923e-8; + boost::gregorian::date expected_gdate; + boost::posix_time::time_duration expected_gtime; - gnav_eph.simplified_satellite_position(60); + boost::posix_time::ptime gtime = gnav_eph.compute_GLONASS_time(7560); + expected_gdate = gtime.date(); + expected_gtime = gtime.time_of_day(); + + // Perform assertions of decoded fields + ASSERT_TRUE(expected_gdate.year() - d.year() < FLT_EPSILON ); + ASSERT_TRUE(expected_gdate.month() - d.month() < FLT_EPSILON ); + ASSERT_TRUE(expected_gdate.day() - d.day() < FLT_EPSILON ); + ASSERT_TRUE(expected_gtime.hours() - t.hours() < FLT_EPSILON ); + ASSERT_TRUE(expected_gtime.minutes() - t.minutes() < FLT_EPSILON ); + ASSERT_TRUE(expected_gtime.seconds() - t.seconds() < FLT_EPSILON ); } diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc index b67b2a696..121ee3c48 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc @@ -1,7 +1,9 @@ /*! * \file glonass_gnav_navigation_message_test.cc * \brief This file implements tests for the decoding of the GLONASS GNAV navigation message + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * * ------------------------------------------------------------------------- @@ -46,12 +48,12 @@ TEST(GlonassGnavNavigationMessageTest, CRCTest) { // Variables declarations in code bool test_result; - std::string str5("0010100100001100000000000000000000000000110011110001100000000000000001100100011000000"); + std::bitset string_bits (std::string ("0010100100001100000000000000000000000000110011110001100000000000000001100100011000000")); Glonass_Gnav_Navigation_Message gnav_nav_message; gnav_nav_message.reset(); // Call function to test - test_result = gnav_nav_message.CRC_test(std::bitset (str5)); + test_result = gnav_nav_message.CRC_test(string_bits); // Check results in unit test assetions ASSERT_TRUE(test_result); @@ -72,20 +74,21 @@ TEST(GlonassGnavNavigationMessageTest, String1Decoder) Glonass_Gnav_Ephemeris gnav_ephemeris; // Fill out ephemeris values for truth - gnav_ephemeris.d_P_1 = static_cast(read_navigation_unsigned(string_bits, P1)); - gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K_HR)) * 3600 + - gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)) * 2e-20; - gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)) * 2e-30; - gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * 2e-11; + gnav_ephemeris.d_P_1 = 0; + gnav_ephemeris.d_t_k = 7560; + gnav_ephemeris.d_VXn = -0.490900039672852; + gnav_ephemeris.d_AXn = 0; + gnav_ephemeris.d_Xn = -11025.6669921875; // Call target test method - gnav_nav_message.string_decoder(str1.c_str()); + gnav_nav_message.string_decoder(const_cast (str1.c_str())); // Perform assertions of decoded fields - ASSERT_TRUE(gnav_ephemeris.d_t_k - gnav_nav_message.gnav_ephemeris.d_t_k < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_VXn - gnav_nav_message.gnav_ephemeris.d_VXn < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_AXn - gnav_nav_message.gnav_ephemeris.d_AXn < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_Xn - gnav_nav_message.gnav_ephemeris.d_Xn < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_P_1 - gnav_nav_message.gnav_ephemeris.d_P_1 < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_t_k - gnav_nav_message.gnav_ephemeris.d_t_k < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_VXn - gnav_nav_message.gnav_ephemeris.d_VXn < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_AXn - gnav_nav_message.gnav_ephemeris.d_AXn < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_Xn - gnav_nav_message.gnav_ephemeris.d_Xn < FLT_EPSILON ); } /*! @@ -103,29 +106,129 @@ TEST(GlonassGnavNavigationMessageTest, String2Decoder) Glonass_Gnav_Ephemeris gnav_ephemeris; // Fill out ephemeris values for truth - gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); - gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); - gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*15*60; - gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* 2e-20; - gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * 2e-30; - gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, X_N)) * 2e-11; + gnav_ephemeris.d_B_n = 0; + gnav_ephemeris.d_P_2 = 1; + gnav_ephemeris.d_t_b = 8100; + gnav_ephemeris.d_VYn = -2.69022750854492; + gnav_ephemeris.d_AYn = 0; + gnav_ephemeris.d_Yn = -11456.7348632812; // Call target test method - gnav_nav_message.string_decoder(str2.c_str()) + gnav_nav_message.string_decoder(const_cast (str2.c_str())); // Perform assertions of decoded fields - ASSERT_TRUE(gnav_ephemeris.d_B_n - gnav_nav_message.gnav_ephemeris.d_B_n < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_P_2 - gnav_nav_message.gnav_ephemeris.d_P_2 < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_t_b - gnav_nav_message.gnav_ephemeris.d_t_b < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_VYn - gnav_nav_message.gnav_ephemeris.d_VYn < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_AYn - gnav_nav_message.gnav_ephemeris.d_AYn < DBL_EPSILON ); - ASSERT_TRUE(gnav_ephemeris.d_Yn - gnav_nav_message.gnav_ephemeris.d_Yn < DBL_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_B_n - gnav_nav_message.gnav_ephemeris.d_B_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_P_2 - gnav_nav_message.gnav_ephemeris.d_P_2 < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_t_b - gnav_nav_message.gnav_ephemeris.d_t_b < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_VYn - gnav_nav_message.gnav_ephemeris.d_VYn < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_AYn - gnav_nav_message.gnav_ephemeris.d_AYn < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_Yn - gnav_nav_message.gnav_ephemeris.d_Yn < FLT_EPSILON ); +} + +/*! + * \brief Testing string decoding for GLONASS GNAV messages + * \test The provided string (str1.....str15) was generated with a version of + * MATLAB GNSS-SDR that the author coded to perform proper decoding of GLONASS + * GNAV signals. The same assumption is to be applied for ephemeris and almanac + * data provided. + */ +TEST(GlonassGnavNavigationMessageTest, String3Decoder) +{ + // Variable declarations + std::string str3("0001110000000001001101001110100011111011010011001101001101110110010011110011100100011"); + Glonass_Gnav_Navigation_Message gnav_nav_message; + Glonass_Gnav_Ephemeris gnav_ephemeris; + + // Fill out ephemeris values for truth + gnav_ephemeris.d_P_3 = 1; + gnav_ephemeris.d_gamma_n = 1.81898940354586e-12; + gnav_ephemeris.d_P = 3; + gnav_ephemeris.d_l3rd_n = 0; + gnav_ephemeris.d_VZn = -1.82016849517822; + gnav_ephemeris.d_AZn = -2.79396772384644e-09; + gnav_ephemeris.d_Zn = 19929.2377929688; + + // Call target test method + gnav_nav_message.string_decoder(const_cast (str3.c_str())); + + // Perform assertions of decoded fields + ASSERT_TRUE(gnav_ephemeris.d_P_3 - gnav_nav_message.gnav_ephemeris.d_P_3 < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_gamma_n - gnav_nav_message.gnav_ephemeris.d_gamma_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_P - gnav_nav_message.gnav_ephemeris.d_P < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_l3rd_n - gnav_nav_message.gnav_ephemeris.d_l3rd_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_VZn - gnav_nav_message.gnav_ephemeris.d_VZn < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_AZn - gnav_nav_message.gnav_ephemeris.d_AZn < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_Zn - gnav_nav_message.gnav_ephemeris.d_Zn < FLT_EPSILON ); +} + +/*! + * \brief Testing string decoding for GLONASS GNAV messages + * \test The provided string (str1.....str15) was generated with a version of + * MATLAB GNSS-SDR that the author coded to perform proper decoding of GLONASS + * GNAV signals. The same assumption is to be applied for ephemeris and almanac + * data provided. + */ +TEST(GlonassGnavNavigationMessageTest, String4Decoder) +{ + // Variable declarations + std::string str4("0010010000101011100100000100000100000000000000000000011000100100001100101010100011101"); + Glonass_Gnav_Navigation_Message gnav_nav_message; + Glonass_Gnav_Ephemeris gnav_ephemeris; + + // Fill out ephemeris values for truth + gnav_ephemeris.d_tau_n = -8.30907374620438e-05; + gnav_ephemeris.d_Delta_tau_n = 9.31322574615479e-10; + gnav_ephemeris.d_E_n = 0; + gnav_ephemeris.d_P_4 = 0; + gnav_ephemeris.d_F_T = 6; + gnav_ephemeris.d_N_T = 268; + gnav_ephemeris.d_n = 21; + gnav_ephemeris.d_M = 1; + + // Call target test method + gnav_nav_message.string_decoder(const_cast (str4.c_str())); + + // Perform assertions of decoded fields + ASSERT_TRUE(gnav_ephemeris.d_tau_n - gnav_nav_message.gnav_ephemeris.d_tau_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_Delta_tau_n - gnav_nav_message.gnav_ephemeris.d_Delta_tau_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_E_n - gnav_nav_message.gnav_ephemeris.d_E_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_P_4 - gnav_nav_message.gnav_ephemeris.d_P_4 < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_F_T - gnav_nav_message.gnav_ephemeris.d_F_T < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_N_T - gnav_nav_message.gnav_ephemeris.d_N_T < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_n - gnav_nav_message.gnav_ephemeris.d_n < FLT_EPSILON ); + ASSERT_TRUE(gnav_ephemeris.d_M - gnav_nav_message.gnav_ephemeris.d_M < FLT_EPSILON ); +} + +/*! + * \brief Testing string decoding for GLONASS GNAV messages + * \test The provided string (str1.....str15) was generated with a version of + * MATLAB GNSS-SDR that the author coded to perform proper decoding of GLONASS + * GNAV signals. The same assumption is to be applied for ephemeris and almanac + * data provided. + */ +TEST(GlonassGnavNavigationMessageTest, String5Decoder) +{ + // Variable declarations + std::string str5("0010100100001100000000000000000000000000110011110001100000000000000001100100011000000"); + Glonass_Gnav_Navigation_Message gnav_nav_message; + Glonass_Gnav_Utc_Model gnav_utc_model; + + // Fill out ephemeris values for truth + gnav_utc_model.d_N_A = 268; + gnav_utc_model.d_tau_c = 9.6391886472702e-08; + gnav_utc_model.d_N_4 = 6; + gnav_utc_model.d_tau_gps = 9.313225746154785e-08; + + // Call target test method + gnav_nav_message.string_decoder(const_cast (str5.c_str())); + + // Perform assertions of decoded fields + ASSERT_TRUE(gnav_utc_model.d_N_A - gnav_nav_message.gnav_utc_model.d_N_A < FLT_EPSILON ); + ASSERT_TRUE(gnav_utc_model.d_tau_c - gnav_nav_message.gnav_utc_model.d_tau_c < FLT_EPSILON ); + ASSERT_TRUE(gnav_utc_model.d_N_4 - gnav_nav_message.gnav_utc_model.d_N_4 < FLT_EPSILON ); + ASSERT_TRUE(gnav_utc_model.d_tau_gps - gnav_nav_message.gnav_utc_model.d_tau_gps < FLT_EPSILON ); } -std::string str2("0001000010001001000001010101100001011001011000000010101100110000001011110000110011110"); -std::string str3("0001110000000001001101001110100011111011010011001101001101110110010011110011100100011"); -std::string str4("0010010000101011100100000100000100000000000000000000011000100100001100101010100011101"); -std::string str5("0010100100001100000000000000000000000000110011110001100000000000000001100100011000000"); std::string str6("0011010100110100001100111100011100001101011000000110101111001000000101100011111011001"); std::string str7("0011101101010001000010000110101111110000101101001011111110101110100010111100010001101"); std::string str8("0100010100111000000001111110001101000000110000001000100111011100001010101111010011010"); From e1d8a7b23a21f5c468b8de58da2d9506830ca437 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 20 Aug 2017 16:13:37 -0700 Subject: [PATCH 35/48] unit-test: Testing RTCM MS1020 Message generation Tests MS1020 message and fixes several bugs that show up while debugging the code. Major changes were introduced due to the bug correction process --- .../glonass_gnav_navigation_message.cc | 25 ++- src/core/system_parameters/rtcm.cc | 190 +++++++++++++----- src/core/system_parameters/rtcm.h | 11 +- .../signal-processing-blocks/pvt/rtcm_test.cc | 72 +++---- .../glonass_gnav_nav_message_test.cc | 4 +- 5 files changed, 191 insertions(+), 111 deletions(-) diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 916b3004a..0b64dc066 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -330,7 +330,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) { case 1: //--- It is string 1 ----------------------------------------------- - gnav_ephemeris.d_P_1 = static_cast(read_navigation_unsigned(string_bits, P1)); + gnav_ephemeris.d_P_1 = (static_cast(read_navigation_unsigned(string_bits, P1)) + 1)*15; gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K_HR)) * 3600 + static_cast(read_navigation_unsigned(string_bits, T_K_MIN)) * 60 + static_cast(read_navigation_unsigned(string_bits, T_K_SEC)) * 30; @@ -338,25 +338,24 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)) * TWO_N30; gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * TWO_N11; - if(gnav_ephemeris.d_P_1 != 0) - { - gnav_ephemeris.d_P_1 = (gnav_ephemeris.d_P_1 + 1)*15; - } flag_ephemeris_str_1 = true; break; case 2: //--- It is string 2 ----------------------------------------------- - gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); - gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); - gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*15*60; - gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* TWO_N20; - gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * TWO_N30; - gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)) * TWO_N11; + if (flag_ephemeris_str_1 == true) + { + gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); + gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); + gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*gnav_ephemeris.d_P_1*60; + gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* TWO_N20; + gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * TWO_N30; + gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)) * TWO_N11; - gnav_ephemeris.d_iode = read_navigation_unsigned(string_bits, T_B); - flag_ephemeris_str_2 = true; + gnav_ephemeris.d_iode = read_navigation_unsigned(string_bits, T_B); + flag_ephemeris_str_2 = true; + } break; diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 662006d92..bce3da0c4 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -316,6 +316,41 @@ long int Rtcm::bin_to_int(const std::string& s) const } +long int Rtcm::bin_to_sint(const std::string& s) const +{ + if(s.length() > 32) + { + LOG(WARNING) << "Cannot convert to a long int"; + return 0; + } + long int reading; + long int sign; + + // Check for sign bit as defined RTCM doc + if(s.substr(0,1).compare("0") == 0) + { + sign = 1; + // Get the magnitude of the value + reading = strtol((s.substr (1)).c_str(), NULL, 2); + } + else + { + sign = -1; + // Get the magnitude of the value + reading = strtol((s.substr (1)).c_str(), NULL, 2); + } + return sign*reading; +} + +// Find the sign for glonass data fields (neg = 1, pos = 0) +static inline unsigned long glo_sgn(double val) +{ + if (val < 0) return 1; // If value is negative return 1 + if (val==0) return 0; // Positive or equal to zero return 0 + return 0; +} + + double Rtcm::bin_to_double(const std::string& s) const { double reading; @@ -1791,7 +1826,7 @@ std::string Rtcm::print_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, DF134.to_string() + DF135.to_string() + DF136.to_string() + - DF137.to_string(); + std::bitset<7>().to_string(); // Reserved bits if (data.length() != 360) { @@ -1828,7 +1863,7 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon unsigned int read_message_length = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 10))); index += 10; - if (read_message_length != 61) + if (read_message_length != 45) // 360 bits = 45 bytes { LOG(WARNING) << " Message MT1020 seems too long (61 bytes expected, " << read_message_length << " received)"; return 1; @@ -1859,7 +1894,8 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon index += 1; if(glonass_gnav_alm_health_ind){} //Avoid comiler warning - glonass_gnav_eph.d_P_1 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); + glonass_gnav_eph.d_P_1 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); + glonass_gnav_eph.d_P_1 = (glonass_gnav_eph.d_P_1+1)*15; index += 2; glonass_gnav_eph.d_t_k += static_cast(Rtcm::bin_to_int(message_bin.substr(index, 5)))*3600; @@ -1875,41 +1911,41 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.d_P_2 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; - glonass_gnav_eph.d_t_b = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 7)))*15.0*60.0; + glonass_gnav_eph.d_t_b = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 7)))*glonass_gnav_eph.d_P_1*60.0; index += 7; // TODO Check for type spec for intS24 - glonass_gnav_eph.d_VXn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 24)))*TWO_N20; + glonass_gnav_eph.d_VXn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 24)))*TWO_N20; index += 24; - glonass_gnav_eph.d_Xn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 27)))*TWO_N11; + glonass_gnav_eph.d_Xn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 27)))*TWO_N11; index += 27; - glonass_gnav_eph.d_AXn = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)))*TWO_N30; + glonass_gnav_eph.d_AXn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 5)))*TWO_N30; index += 5; - glonass_gnav_eph.d_VYn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 24)))*TWO_N20; + glonass_gnav_eph.d_VYn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 24)))*TWO_N20; index += 24; - glonass_gnav_eph.d_Yn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 27)))*TWO_N11; + glonass_gnav_eph.d_Yn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 27)))*TWO_N11; index += 27; - glonass_gnav_eph.d_AYn = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)))*TWO_N30; + glonass_gnav_eph.d_AYn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 5)))*TWO_N30; index += 5; - glonass_gnav_eph.d_VZn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 24)))*TWO_N20; + glonass_gnav_eph.d_VZn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 24)))*TWO_N20; index += 24; - glonass_gnav_eph.d_Zn = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 27)))*TWO_N11; + glonass_gnav_eph.d_Zn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 27)))*TWO_N11; index += 27; - glonass_gnav_eph.d_AZn = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)))*TWO_N30; + glonass_gnav_eph.d_AZn = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 5)))*TWO_N30; index += 5; glonass_gnav_eph.d_P_3 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; - glonass_gnav_eph.d_gamma_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11)))*TWO_N30; + glonass_gnav_eph.d_gamma_n = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 11)))*TWO_N30; index += 11; glonass_gnav_eph.d_P = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 2))); @@ -1918,10 +1954,10 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.d_l3rd_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); index += 1; - glonass_gnav_eph.d_tau_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 22)))* TWO_N30; + glonass_gnav_eph.d_tau_n = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 22)))* TWO_N30; index += 22; - glonass_gnav_eph.d_Delta_tau_n = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 5)))* TWO_N30; + glonass_gnav_eph.d_Delta_tau_n = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 5)))* TWO_N30; index += 5; glonass_gnav_eph.d_E_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); @@ -1947,13 +1983,13 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_utc_model.d_N_A = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 11))); index += 11; - glonass_gnav_utc_model.d_tau_c = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 32)))* TWO_N31; + glonass_gnav_utc_model.d_tau_c = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 32)))* TWO_N31; index += 32; glonass_gnav_utc_model.d_N_4 = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); index += 5; - glonass_gnav_utc_model.d_tau_gps = static_cast(Rtcm::bin_to_int(message_bin.substr(index, 22)))* TWO_N30; + glonass_gnav_utc_model.d_tau_gps = static_cast(Rtcm::bin_to_sint(message_bin.substr(index, 22)))* TWO_N30; index += 22; glonass_gnav_eph.d_l5th_n = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); @@ -2306,11 +2342,12 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, bool divergence_free, bool more_messages) { - std::map::const_iterator gnss_synchro_iter; - std::string sys(gnss_synchro_iter->second.System, 1); + // Find first element in observables block and define type of message + std::map::const_iterator observables_iter = observables.begin(); + std::string sys(observables_iter->second.System, 1); + Rtcm::set_DF002(msg_number); Rtcm::set_DF003(ref_id); - Rtcm::set_DF393(more_messages); Rtcm::set_DF409(0); // Issue of Data Station. 0: not utilized std::bitset<7> DF001_ = std::bitset<7>("0000000"); @@ -4303,7 +4340,8 @@ int Rtcm::set_DF105(unsigned int glonass_gnav_alm_health_ind) int Rtcm::set_DF106(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - unsigned int P_1 = static_cast(std::round(glonass_gnav_eph.d_P_1)); + // Convert the value from (15, 30, 45, 60) to (00, 01, 10, 11) + unsigned int P_1 = static_cast(std::round(glonass_gnav_eph.d_P_1/15.0 -1.0)); DF106 = std::bitset<2>(P_1); return 0; } @@ -4312,7 +4350,22 @@ int Rtcm::set_DF106(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) int Rtcm::set_DF107(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { // TODO Need to fix this here, bit position has a given order - DF107 = std::bitset<12>(glonass_gnav_eph.d_t_k); + unsigned int hrs = 0; + unsigned int min = 0; + unsigned int sec = 0; + unsigned int tk = 0; + tk = static_cast(glonass_gnav_eph.d_t_k); + hrs = tk/3600; + min = (tk - hrs*3600)/60; + sec = (tk - hrs*3600 -min*60)/60; + + std::string _hrs = std::bitset< 5 >( hrs ).to_string(); // string conversion + std::string _min = std::bitset< 6 >( min ).to_string(); // string conversion + std::string _sec = std::bitset< 1 >( sec ).to_string(); // string conversion + + // Set hrs, min, sec in designed bit positions + DF107 = std::bitset<12>(_hrs + _min + _sec); + return 0; } @@ -4333,7 +4386,7 @@ int Rtcm::set_DF109(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) int Rtcm::set_DF110(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - unsigned int t_b = static_cast(std::round(glonass_gnav_eph.d_t_b)); + unsigned int t_b = static_cast(std::round(glonass_gnav_eph.d_t_b/(glonass_gnav_eph.d_P_1*60))); DF110 = std::bitset<7>(t_b); return 0; } @@ -4341,79 +4394,109 @@ int Rtcm::set_DF110(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) int Rtcm::set_DF111(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int VXn = static_cast(std::round(glonass_gnav_eph.d_VXn/TWO_N20)); - DF111 = std::bitset<24>(VXn); + int VXn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_VXn/TWO_N20))); + unsigned int VXn_sgn = glo_sgn(glonass_gnav_eph.d_VXn); + + DF111 = std::bitset<24>(VXn_mag); + DF111.set(23,VXn_sgn); return 0; } int Rtcm::set_DF112(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int Xn = static_cast(std::round(glonass_gnav_eph.d_Xn/TWO_N11)); - DF112 = std::bitset<27>(Xn); + int Xn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_Xn/TWO_N11))); + unsigned int Xn_sgn = glo_sgn(glonass_gnav_eph.d_Xn); + + DF112 = std::bitset<27>(Xn_mag); + DF112.set(26,Xn_sgn); return 0; } int Rtcm::set_DF113(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int AXn = static_cast(std::round(glonass_gnav_eph.d_AXn/TWO_N11)); - DF113 = std::bitset<5>(AXn); + int AXn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_AXn/TWO_N30))); + unsigned int AXn_sgn = glo_sgn(glonass_gnav_eph.d_AXn); + + DF113 = std::bitset<5>(AXn_mag); + DF113.set(4,AXn_sgn); return 0; } int Rtcm::set_DF114(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int VYn = static_cast(std::round(glonass_gnav_eph.d_VYn/TWO_N20)); - DF114 = std::bitset<24>(VYn); + int VYn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_VYn/TWO_N20))); + unsigned int VYn_sgn = glo_sgn(glonass_gnav_eph.d_VYn); + + DF114 = std::bitset<24>(VYn_mag); + DF114.set(23,VYn_sgn); return 0; } int Rtcm::set_DF115(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int Yn = static_cast(std::round(glonass_gnav_eph.d_Yn/TWO_N11)); - DF115 = std::bitset<27>(Yn); + int Yn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_Yn/TWO_N11))); + unsigned int Yn_sgn = glo_sgn(glonass_gnav_eph.d_Yn); + + DF115 = std::bitset<27>(Yn_mag); + DF115.set(26,Yn_sgn); return 0; } int Rtcm::set_DF116(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int AYn = static_cast(std::round(glonass_gnav_eph.d_AYn/TWO_N11)); - DF116 = std::bitset<5>(AYn); + int AYn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_AYn/TWO_N30))); + unsigned int AYn_sgn = glo_sgn(glonass_gnav_eph.d_AYn); + + DF116 = std::bitset<5>(AYn_mag); + DF116.set(4,AYn_sgn); return 0; } + int Rtcm::set_DF117(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int VZn = static_cast(std::round(glonass_gnav_eph.d_VZn/TWO_N20)); - DF117 = std::bitset<24>(VZn); + int VZn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_VZn/TWO_N20))); + unsigned int VZn_sgn = glo_sgn(glonass_gnav_eph.d_VZn); + + DF117 = std::bitset<24>(VZn_mag); + DF117.set(23,VZn_sgn); return 0; } + int Rtcm::set_DF118(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int Zn = static_cast(std::round(glonass_gnav_eph.d_Zn/TWO_N11)); - DF118 = std::bitset<27>(Zn); + int Zn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_Zn/TWO_N11))); + unsigned int Zn_sgn = glo_sgn(glonass_gnav_eph.d_Zn); + + DF118 = std::bitset<27>(Zn_mag); + DF118.set(26,Zn_sgn); return 0; } + int Rtcm::set_DF119(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int AZn = static_cast(std::round(glonass_gnav_eph.d_AZn/TWO_N11)); - DF119 = std::bitset<5>(AZn); + int AZn_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_AZn/TWO_N30))); + unsigned int AZn_sgn = glo_sgn(glonass_gnav_eph.d_AZn); + + DF119 = std::bitset<5>(AZn_mag); + DF119.set(4,AZn_sgn); return 0; } int Rtcm::set_DF120(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - unsigned int P3 = static_cast(std::round(glonass_gnav_eph.d_P_3/TWO_N11)); + unsigned int P3 = static_cast(std::round(glonass_gnav_eph.d_P_3)); DF120 = std::bitset<1>(P3); return 0; } @@ -4421,8 +4504,11 @@ int Rtcm::set_DF120(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) int Rtcm::set_DF121(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int gamma_n = static_cast(std::round(glonass_gnav_eph.d_gamma_n/TWO_N40)); - DF121 = std::bitset<11>(gamma_n); + int gamma_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_gamma_n/TWO_N40))); + unsigned int gamma_sgn = glo_sgn(glonass_gnav_eph.d_gamma_n); + + DF121 = std::bitset<11>(gamma_mag); + DF121.set(10,gamma_sgn); return 0; } @@ -4445,16 +4531,22 @@ int Rtcm::set_DF123(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) int Rtcm::set_DF124(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int tau_n = static_cast(std::round(glonass_gnav_eph.d_tau_n/TWO_N30)); - DF124 = std::bitset<22>(tau_n); + int tau_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_tau_n/TWO_N30))); + unsigned int tau_sgn = glo_sgn(glonass_gnav_eph.d_tau_n); + + DF124 = std::bitset<22>(tau_mag); + DF124.set(21,tau_sgn); return 0; } int Rtcm::set_DF125(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - int delta_tau_n = static_cast(std::round(glonass_gnav_eph.d_Delta_tau_n)); - DF125 = std::bitset<5>(delta_tau_n); + int delta_tau_mag = static_cast(std::round(fabs(glonass_gnav_eph.d_Delta_tau_n/TWO_N30))); + unsigned int delta_tau_sgn = glo_sgn(glonass_gnav_eph.d_Delta_tau_n); + + DF125 = std::bitset<5>(delta_tau_mag); + DF125.set(4,delta_tau_sgn); return 0; } diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index 3ba745217..f19b71add 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -348,9 +348,16 @@ public: unsigned long int bin_to_uint(const std::string & s) const; //(); @@ -282,53 +283,32 @@ TEST(RtcmTest, MT1020) bool expected_true = true; // Objects to populate the ephemeris and utc fields - Glonass_Gnav_Ephemeris gnav_eph = Glonass_Gnav_Ephemeris(); + Glonass_Gnav_Ephemeris gnav_ephemeris = Glonass_Gnav_Ephemeris(); Glonass_Gnav_Utc_Model gnav_utc_model = Glonass_Gnav_Utc_Model(); // Objects read, used for comparison - Glonass_Gnav_Ephemeris gnav_eph_read = Glonass_Gnav_Ephemeris(); + Glonass_Gnav_Ephemeris gnav_ephemeris_read = Glonass_Gnav_Ephemeris(); Glonass_Gnav_Utc_Model gnav_utc_model_read = Glonass_Gnav_Utc_Model(); - glonass_gnav_eph.i_satellite_slot_number = 3; - gnav_ephemeris.d_P_1 = 0; + // Perform data read and print of special values types + gnav_ephemeris.d_P_1 = 15; + // Bit distribution per fields gnav_ephemeris.d_t_k = 7560; + // Glonass signed values gnav_ephemeris.d_VXn = -0.490900039672852; - gnav_ephemeris.d_AXn = 0; - gnav_ephemeris.d_Xn = -11025.6669921875; - gnav_ephemeris.d_B_n = 0; - gnav_ephemeris.d_P_2 = 1; + // Bit distribution per fields dependant on other factors gnav_ephemeris.d_t_b = 8100; - gnav_ephemeris.d_VYn = -2.69022750854492; - gnav_ephemeris.d_AYn = 0; - gnav_ephemeris.d_Yn = -11456.7348632812; + // Binary flag representation gnav_ephemeris.d_P_3 = 1; - gnav_ephemeris.d_gamma_n = 1.81898940354586e-12; - gnav_ephemeris.d_P = 3; - gnav_ephemeris.d_l3rd_n = 0; - gnav_ephemeris.d_VZn = -1.82016849517822; - gnav_ephemeris.d_AZn = -2.79396772384644e-09; - gnav_ephemeris.d_Zn = 19929.2377929688; - gnav_ephemeris.d_tau_n = -8.30907374620438e-05; - gnav_ephemeris.d_Delta_tau_n = 9.31322574615479e-10; - gnav_ephemeris.d_E_n = 0; - gnav_ephemeris.d_P_4 = 0; - gnav_ephemeris.d_F_T = 6; - gnav_ephemeris.d_N_T = 268; - gnav_ephemeris.d_n = 21; - gnav_ephemeris.d_M = 1; - gnav_utc_model.d_N_A = 268; - gnav_utc_model.d_tau_c = 9.6391886472702e-08; - gnav_utc_model.d_N_4 = 6; - gnav_utc_model.d_tau_gps = 9.313225746154785e-08; - std::string tx_msg = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); + std::string tx_msg = rtcm->print_MT1020(gnav_ephemeris, gnav_utc_model); - EXPECT_EQ(0, rtcm->read_MT1020(tx_msg, glonass_gnav_eph_read, glonass_gnav_utc_model_read)); - EXPECT_EQ(3, glonass_gnav_eph_read.i_satellite_slot_number); - EXPECT_DOUBLE_EQ(4, glonass_gnav_eph_read.d_t_b); - EXPECT_DOUBLE_EQ( 2.0 * E_LSB, glonass_gnav_eph_read.d_E_n); - EXPECT_DOUBLE_EQ( 5, glonass_gnav_utc_model_read.d_tau_gps); - EXPECT_EQ(expected_true, glonass_gnav_eph_read.d_l3rd_n); - EXPECT_EQ(1, rtcm->read_MT1020(rtcm->bin_to_binary_data(rtcm->hex_to_bin("FFFFFFFFFFF")), glonass_gnav_eph_read, glonass_gnav_utc_model_read)); + EXPECT_EQ(0, rtcm->read_MT1020(tx_msg, gnav_ephemeris_read, gnav_utc_model_read)); + EXPECT_EQ(gnav_ephemeris.d_P_1, gnav_ephemeris_read.d_P_1); + EXPECT_TRUE(gnav_ephemeris.d_t_b - gnav_ephemeris_read.d_t_b < FLT_EPSILON); + EXPECT_TRUE( gnav_ephemeris.d_VXn - gnav_ephemeris_read.d_VXn < FLT_EPSILON); + EXPECT_TRUE( gnav_ephemeris.d_t_k - gnav_ephemeris.d_t_k < FLT_EPSILON); + EXPECT_EQ(gnav_ephemeris.d_P_3, gnav_ephemeris_read.d_P_3); + EXPECT_EQ(1, rtcm->read_MT1020(rtcm->bin_to_binary_data(rtcm->hex_to_bin("FFFFFFFFFFF")), gnav_ephemeris_read, gnav_utc_model_read)); } @@ -409,12 +389,12 @@ TEST(RtcmTest, MSMCell) gnss_synchro5.System = *gps.c_str(); gnss_synchro6.System = *glo.c_str(); - std::memcpy(static_cast(gnss_synchro.Signal), x5.c_str(), 3); - std::memcpy(static_cast(gnss_synchro2.Signal), s2.c_str(), 3); - std::memcpy(static_cast(gnss_synchro3.Signal), c1.c_str(), 3); - std::memcpy(static_cast(gnss_synchro4.Signal), x5.c_str(), 3); - std::memcpy(static_cast(gnss_synchro5.Signal), c1.c_str(), 3); - std::memcpy(static_cast(gnss_synchro6.Signal), c1.c_str(), 3); + std::memcpy((void*)gnss_synchro.Signal, x5.c_str(), 3); + std::memcpy((void*)gnss_synchro2.Signal, s2.c_str(), 3); + std::memcpy((void*)gnss_synchro3.Signal, c1.c_str(), 3); + std::memcpy((void*)gnss_synchro4.Signal, x5.c_str(), 3); + std::memcpy((void*)gnss_synchro5.Signal, c1.c_str(), 3); + std::memcpy((void*)gnss_synchro6.Signal, c1.c_str(), 3); gnss_synchro.Pseudorange_m = 20000000.0; gnss_synchro2.Pseudorange_m = 20001010.0; @@ -445,7 +425,7 @@ TEST(RtcmTest, MSMCell) std::string MSM1 = rtcm->print_MSM_1(gps_eph, {}, gal_eph, - glo_gnav_eph, + {}, obs_time, pseudoranges, ref_id, @@ -473,7 +453,7 @@ TEST(RtcmTest, MSMCell) std::string MSM1_2 = rtcm->print_MSM_1(gps_eph, {}, gal_eph, - glo_gnav_eph, + {}, obs_time, pseudoranges2, ref_id, @@ -488,7 +468,7 @@ TEST(RtcmTest, MSMCell) Gnss_Synchro gnss_synchro7; gnss_synchro7.PRN = 10; gnss_synchro7.System = *gps.c_str(); - std::memcpy(static_cast(gnss_synchro7.Signal), s2.c_str(), 3); + std::memcpy((void*)gnss_synchro7.Signal, s2.c_str(), 3); gnss_synchro7.Pseudorange_m = 24000000.0; std::map pseudoranges3; diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc index 121ee3c48..df7d740fd 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc @@ -74,7 +74,7 @@ TEST(GlonassGnavNavigationMessageTest, String1Decoder) Glonass_Gnav_Ephemeris gnav_ephemeris; // Fill out ephemeris values for truth - gnav_ephemeris.d_P_1 = 0; + gnav_ephemeris.d_P_1 = 15; gnav_ephemeris.d_t_k = 7560; gnav_ephemeris.d_VXn = -0.490900039672852; gnav_ephemeris.d_AXn = 0; @@ -114,6 +114,8 @@ TEST(GlonassGnavNavigationMessageTest, String2Decoder) gnav_ephemeris.d_Yn = -11456.7348632812; // Call target test method + gnav_nav_message.flag_ephemeris_str_1 = true; + gnav_nav_message.gnav_ephemeris.d_P_1 = 15; gnav_nav_message.string_decoder(const_cast (str2.c_str())); // Perform assertions of decoded fields From 305a81a4131ac1eea0a0edbe71654f0fc529444e Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 20 Aug 2017 21:23:57 -0700 Subject: [PATCH 36/48] tel_dec: Fixing GLONASS GNAV Telemetry Decoder Fixes GLONASS L1 CA telemetry decoder and creating connections with observables blocks before PVT computation. Fixes documentation and code style in the block --- .../glonass_l1_ca_telemetry_decoder.cc | 4 +-- .../glonass_l1_ca_telemetry_decoder.h | 1 + .../glonass_l1_ca_telemetry_decoder_cc.cc | 25 ++++++++++++----- .../glonass_l1_ca_telemetry_decoder_cc.h | 27 ++++++++++--------- src/core/receiver/gnss_block_factory.cc | 13 +++++++++ 5 files changed, 49 insertions(+), 21 deletions(-) diff --git a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc index 868fb3304..924efd2da 100644 --- a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.cc @@ -2,6 +2,7 @@ * \file glonass_l1_ca_telemetry_decoder.cc * \brief Implementation of an adapter of a GLONASS L1 C/A NAV data decoder block * to a TelemetryDecoderInterface + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- @@ -36,7 +37,6 @@ #include "concurrent_queue.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_almanac.h" -#include "glonass_gnav_iono.h" #include "glonass_gnav_utc_model.h" #include "configuration_interface.h" @@ -55,7 +55,7 @@ GlonassL1CaTelemetryDecoder::GlonassL1CaTelemetryDecoder(ConfigurationInterface* dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); // make telemetry decoder object - telemetry_decoder_ = glonass_l1_ca_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me + telemetry_decoder_ = glonass_l1_ca_make_telemetry_decoder_cc(satellite_, dump_); DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; DLOG(INFO) << "global navigation message queue assigned to telemetry_decoder ("<< telemetry_decoder_->unique_id() << ")"; diff --git a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h index d9d883a71..e79f586c9 100644 --- a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h @@ -2,6 +2,7 @@ * \file glonass_l1_ca_telemetry_decoder.h * \brief Interface of an adapter of a GLONASS L1 C/A NAV data decoder block * to a TelemetryDecoderInterface + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index 6318977f5..c0273edaf 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -1,8 +1,9 @@ /*! - * \file galileo_e1b_telemetry_decoder_cc.cc - * \brief Implementation of a Galileo INAV message demodulator block - * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com - * \author Javier Arribas 2013. jarribas(at)cttc.es + * \file glonass_l1_ca_telemetry_decoder_cc.cc + * \brief Implementation of an adapter of a GLONASS L1 C/A NAV data decoder block + * to a TelemetryDecoderInterface + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- * @@ -30,7 +31,7 @@ */ -#include "galileo_e1b_telemetry_decoder_cc.h" +#include "glonass_l1_ca_telemetry_decoder_cc.h" #include #include #include @@ -67,7 +68,7 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "Initializing GLONASS L1CA TELEMETRY PROCESSING"; + LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY PROCESSING"; // TODO. WHAT IS THIS? d_samples_per_symbol = ( GLONASS_L1_CODE_CHIP_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS ) / GLONASS_L1_CA_SYMBOL_RATE_BPS; @@ -115,7 +116,17 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( glonass_l1_ca_telemetry_decoder_cc::~glonass_l1_ca_telemetry_decoder_cc() { delete d_preambles_symbols; - d_dump_file.close(); + if(d_dump_file.is_open() == true) + { + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } + } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h index bfd9d975e..eab2136e2 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h @@ -1,6 +1,8 @@ /*! * \file glonass_l1_ca_telemetry_decoder_cc.h - * \brief Interface of a GLONASS GNAV message demodulator block + * \brief Implementation of an adapter of a GLONASS L1 C/A NAV data decoder block + * to a TelemetryDecoderInterface + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- @@ -40,7 +42,6 @@ #include "glonass_gnav_navigation_message.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_almanac.h" -#include "glonass_gnav_iono.h" #include "glonass_gnav_utc_model.h" #include "gnss_synchro.h" @@ -53,15 +54,17 @@ typedef boost::shared_ptr glonass_l1_ca_tele glonass_l1_ca_telemetry_decoder_cc_sptr glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); /*! - * \brief This class implements a block that decodes the GNAV data defined in GLONASS ICD + * \brief This class implements a block that decodes the GNAV data defined in GLONASS ICD v5.1 + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD * */ class glonass_l1_ca_telemetry_decoder_cc : public gr::block { public: - ~glonass_l1_ca_telemetry_decoder_cc(); - void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN - void set_channel(int channel); //!< Set receiver's channel + ~glonass_l1_ca_telemetry_decoder_cc(); //!< Class destructor + void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN + void set_channel(int channel); //!< Set receiver's channel /*! * \brief This is where all signal processing takes place @@ -74,7 +77,7 @@ private: glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); glonass_l1_ca_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); - void decode_word(double *symbols,int frame_length); + void decode_word(double *symbols); //!< Preamble decoding unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS]; @@ -91,11 +94,11 @@ private: unsigned int d_stat; //!< Status of decoder bool d_flag_frame_sync; //!< Indicate when a frame sync is achieved bool d_flag_parity; //!< Flag indicating when parity check was achieved (crc check) - bool d_flag_preamble; - int d_CRC_error_counter; - bool flag_TOW_set; //!< - double delta_t; //!< GPS-GLONASS time offset - + bool d_flag_preamble; //!< Flag indicating when preamble was found + int d_CRC_error_counter; //!< Number of failed CRC operations + bool flag_TOW_set; //!< Indicates when time of week is set + double delta_t; //!< GPS-GLONASS time offset + //!< Navigation Message variable Glonass_Gnav_Navigation_Message d_nav; diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 7cba2a649..1f9c73529 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -92,6 +92,7 @@ #include "gps_l2c_telemetry_decoder.h" #include "galileo_e1b_telemetry_decoder.h" #include "galileo_e5a_telemetry_decoder.h" +#include "glonass_l1_ca_telemetry_decoder.h" #include "sbas_l1_telemetry_decoder.h" #include "hybrid_observables.h" #include "rtklib_pvt.h" @@ -1203,6 +1204,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GLONASS_L1_CA_Telemetry_Decoder") == 0) + { + std::unique_ptr block_(new GlonassL1CaTelemetryDecoder(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } // OBSERVABLES ----------------------------------------------------------------- else if ((implementation.compare("Hybrid_Observables") == 0) || (implementation.compare("GPS_L1_CA_Observables") == 0) || (implementation.compare("GPS_L2C_Observables") == 0) || @@ -1347,6 +1354,12 @@ std::unique_ptr GNSSBlockFactory::GetAcqBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GLONASS_L1_CA_Telemetry_Decoder") == 0) + { + std::unique_ptr block_(new GlonassL1CaTelemetryDecoder(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else { // Log fatal. This causes execution to stop. From 85f7e333bb255cb17b87379b90d6cabdd758761b Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 23 Aug 2017 15:16:07 -0700 Subject: [PATCH 37/48] bug_fix: Fixes bugs in telemetry decoding interface Fixes several bugs with the telemetry decoder interface and clean up the code with unused methods and members of the ephemeris object --- src/algorithms/PVT/libs/rinex_printer.cc | 63 +- .../glonass_l1_ca_telemetry_decoder_cc.cc | 86 +-- .../glonass_l1_ca_telemetry_decoder_cc.h | 3 +- src/core/system_parameters/GLONASS_L1_CA.h | 15 +- .../glonass_gnav_ephemeris.cc | 661 ------------------ .../glonass_gnav_ephemeris.h | 48 -- .../glonass_gnav_utc_model.h | 2 + 7 files changed, 80 insertions(+), 798 deletions(-) diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index facaa6f5e..06508ee75 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -633,6 +633,8 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning + //Avoid compiler warning, there is not time system correction between Galileo and GLONASS + if(galileo_almanac.A_0G_10){} std::string line; stringVersion = "3.02"; version = 3; @@ -2047,6 +2049,8 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning + //Avoid compiler warning, there is not time system correction between Galileo and GLONASS + if(galileo_almanac.A_0G_10){} std::vector data; std::string line_aux; @@ -3035,7 +3039,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapd_preambles_bits, (unsigned short int*)preambles_bits, GLONASS_GNAV_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int)); // preamble bits to sampled symbols d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble); int n = 0; - for (int i = 0; i < GLONASS_GNAV_PREAMBLE_LENGTH_BITS; i++) + for (int i = 0; i < GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT; i++) { for (unsigned int j = 0; j < d_samples_per_symbol; j++) { @@ -130,40 +132,26 @@ glonass_l1_ca_telemetry_decoder_cc::~glonass_l1_ca_telemetry_decoder_cc() } -void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols,int frame_length) +void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int frame_length) { - double page_part_symbols_deint[frame_length]; - - // 2. Viterbi decoder - // 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder) - // 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180� - for (int i = 0; i < frame_length; i++) + // 1. Transform from symbols to bits + std::string frame_string; + for(int i = 0; i < (frame_length); i++) { - if ((i + 1) % 2 == 0) + if (frame_symbols[i] > 0) { - page_part_symbols_deint[i] = -page_part_symbols_deint[i]; - } - } - - int page_part_bits[frame_length/2]; - viterbi_decoder(page_part_symbols_deint, page_part_bits); - - // 3. Call the Galileo page decoder - std::string page_String; - for(int i = 0; i < (frame_length/2); i++) - { - if (page_part_bits[i] > 0) - { - page_String.push_back('1'); + frame_string.push_back('1'); } else { - page_String.push_back('0'); + frame_string.push_back('0'); } } + // 2. Call the GLONASS GNAV string decoder d_nav.decode_string(page_String); + // 3. Check operation executed correctly if(d_nav.flag_CRC_test == true) { LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite; @@ -175,14 +163,11 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite; } - - // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) { // get object for this SV (mandatory) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } @@ -196,19 +181,10 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols { std::shared_ptr tmp_obj= std::make_shared(d_nav.get_almanac()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - //debug - std::cout << "GLONASS GNAV almanac received!" << std::endl; - DLOG(INFO) << "Current parameters:"; - DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; - DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; - delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); - DLOG(INFO) << "delta_t=" << delta_t << "[s]"; } } - - int glonass_l1_ca_telemetry_decoder_cc::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) { @@ -226,7 +202,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib consume_each(1); d_flag_preamble = false; - unsigned int required_symbols=GLONASS_GNAV_PAGE_SYMBOLS+d_symbols_per_preamble; + unsigned int required_symbols=GLONASS_GNAV_FRAME_BITS+d_symbols_per_preamble; if (d_symbol_history.size()>required_symbols) { @@ -283,7 +259,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib // NEW GLONASS string received // 0. fetch the symbols into an array int frame_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; - double page_part_symbols[frame_length]; + double frame_symbols[frame_length]; //******* SYMBOL TO BIT ******* if (d_symbol_history.at(0).Flag_valid_symbol_output == true) @@ -342,36 +318,20 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) //update TOW at the preamble instant { - if(d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) - { - //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_5 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; - d_nav.flag_TOW_5 = false; - } + d_TOW_at_current_symbol = floor((d_nav.d_TOW + 2*GLONASS_L1_CA_CODE_PERIOD + GLONASS_CA_PREAMBLE_DURATION_S)*1000.0)/1000.0; - else if(d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) - { - //TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_6 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD;//-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; - d_nav.flag_TOW_6 = false; - } - else - { - //this page has no timing information - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; - } } else //if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L1_CA_CODE_PERIOD; } //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) - if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived - { - delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); - } + // if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived + // { + // delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); + // } if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h index eab2136e2..5e4ff6df7 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h @@ -77,12 +77,13 @@ private: glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); glonass_l1_ca_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); - void decode_word(double *symbols); + void decode_string(double *symbols, int frame_length); //!< Preamble decoding unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS]; int *d_preambles_symbols; unsigned int d_samples_per_symbol; + unsigned int d_samples_per_preamble_symbol; int d_symbols_per_preamble; //!< Storage for incoming data diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 3a93d0def..3b03a58e5 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -94,13 +94,14 @@ const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time ( const int GLONASS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING -#define GLONASS_CA_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0} -const int GLONASS_CA_PREAMBLE_LENGTH_BITS = 30; -const int GLONASS_CA_PREAMBLE_LENGTH_SYMBOLS = 300; -const double GLONASS_CA_PREAMBLE_DURATION_S = 0.3; -const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] -const int GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT = 10; -const int GLONASS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_CA_TELEMETRY_RATE_BITS_SECOND*GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s] +#define GLONASS_GNAV_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0} +const int GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30; +const int GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300; +const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3; +const int GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] +const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10; +const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10; +const int GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND*GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s] const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame in the GNAV message 15 strings*(85 data bits + 30 time mark bits)[bits] const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index 3b66f7a12..f5bb76eab 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -81,18 +81,6 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() d_tau_c = 0.0; d_TOW = 0.0; // tow of the start of frame d_WN = 0.0; // week number of the start of frame - // satellite positions - d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity - d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration - d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] } @@ -107,122 +95,6 @@ boost::posix_time::ptime Glonass_Gnav_Ephemeris::compute_GLONASS_time(const doub } -void Glonass_Gnav_Ephemeris::gravitational_perturbations() -{ - double T = 0.0; - double sigma_days = 0.0; - double tau_prime = 0.0; - double Omega_moon = 0.0; - double q_moon = 0.0; - double q_sun = 0.0; - - double xi_star = 0.0; - double eta_star = 0.0; - double zeta_star = 0.0; - double E_moon = 0.0; - double E_sun = 0.0; - double xi_11 = 0.0; - double xi_12 = 0.0; - double eta_11 = 0.0; - double eta_12 = 0.0; - double zeta_11 = 0.0; - double zeta_12 = 0.0; - - double sin_theta_moon = 0.0; - double cos_theta_moon = 0.0; - double theta_moon = 0.0; - double xi_moon_e = 0.0; - double eta_moon_e = 0.0; - double zeta_moon_e = 0.0; - double r_moon_e = 0.0; - - double sin_theta_sun = 0.0; - double cos_theta_sun = 0.0; - double theta_sun = 0.0; - double xi_sun_e = 0.0; - double eta_sun_e = 0.0; - double zeta_sun_e = 0.0; - double r_sun_e = 0.0; - - double mu_bar_moon = 0.0; - double x_bar_moon = 0.0; - double y_bar_moon = 0.0; - double z_bar_moon = 0.0; - double Delta_o_moon = 0.0; - double mu_bar_sun = 0.0; - double x_bar_sun = 0.0; - double y_bar_sun = 0.0; - double z_bar_sun = 0.0; - double Delta_o_sun = 0.0; - - - double t_e = 0.0; - - // fabs(tk)) - { - // if there is more time left to integrate... - if(fmod(tk,tau) != 0) - { - tau = fmod(tk,tau); - } - else // otherwise make a zero step. - { - tau = 0; - } - } - - // Runge-Kutta Integration Stage K1 - r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0); - r0_2 = r0*r0; - r0_3 = r0*r0*r0; - r0_5 = r0*r0*r0*r0*r0; - - dVx_1 = - GLONASS_GM / r0_3 * x_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * x_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_0 + Ax_0; - dVy_1 = - GLONASS_GM / r0_3 * y_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * y_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_0 + Ay_0; - dVz_1 = - GLONASS_GM / r0_3 * z_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * z_0 * (3 - 5 * (z_0*z_0) / r0_2) + Az_0; - - dx_1 = Vx_0; - dy_1 = Vy_0; - dz_1 = Vz_0; - - // Runge-Kutta Integration Stage K2 - Vx_1 = Vx_0 + 1/2 * tau * dVx_1; - Vy_1 = Vy_0 + 1/2 * tau * dVy_1; - Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - - x_1 = x_0 + 1/2 * tau * dx_1; - y_1 = y_0 + 1/2 * tau * dy_1; - z_1 = z_0 + 1/2 * tau * dz_1; - - r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); - r1_2 = r1*r1; - r1_3 = r1*r1*r1; - r1_5 = r1*r1*r1*r1*r1; - - dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0; - dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0; - dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; - - dx_2 = Vx_1; - dy_2 = Vy_1; - dz_2 = Vz_1; - - // Runge-Kutta Integration Stage K2 - Vx_2 = Vx_0 + 1/2 * tau * dVx_2; - Vy_2 = Vy_0 + 1/2 * tau * dVy_2; - Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - - x_2 = x_0 + 1/2 * tau * dx_2; - y_2 = y_0 + 1/2 * tau * dy_2; - z_2 = z_0 + 1/2 * tau * dz_2; - - r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); - r2_2 = r2*r2; - r2_3 = r2*r2*r2; - r2_5 = r2*r2*r2*r2*r2; - - - dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0; - dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0; - dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; - - dx_3 = Vx_2; - dy_3 = Vy_2; - dz_3 = Vz_2; - - // Runge-Kutta Integration Stage K3 - Vx_3 = Vx_0 + tau * dVx_3; - Vy_3 = Vy_0 + tau * dVy_3; - Vz_3 = Vz_0 + tau * dVz_3; - - x_3 = x_0 + tau * dx_3; - y_3 = y_0 + tau * dy_3; - z_3 = z_0 + tau * dz_3; - - r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); - r3_2 = r3*r3; - r3_3 = r3*r3*r3; - r3_5 = r3*r3*r3*r3*r3; - - dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0; - dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0; - dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; - - dx_4 = Vx_3; - dy_4 = Vy_3; - dz_4 = Vz_3; - - // Final results showcased here - Vx_0 = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); - Vy_0 = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); - Vz_0 = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); - - x_0 = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); - y_0 = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); - z_0 = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); - - } - - // Reasign position, velocities and accelerations for next integration - d_satpos_X = x_0; - d_satpos_Y = y_0; - d_satpos_Z = z_0; - - d_satvel_X = Vx_0; - d_satvel_Y = Vy_0; - d_satvel_Z = Vz_0; - - d_satacc_X = d_AXn; // No change in accelerations reported over interval - d_satacc_Y = d_AYn; // No change in accelerations reported over interval - d_satacc_Z = d_AZn; // No change in accelerations reported over interval - - // Time from ephemeris reference clock - //tk = check_t(transmitTime - d_Toc); - - //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; - - /* relativity correction */ - //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); - - return 0; -} - - -double Glonass_Gnav_Ephemeris::satellite_position(double transmitTime) -{ - double dt = 0.0; - double tk = 0.0; - double tau = 0.0; - int numberOfIntegrations = 0; - - // RK 1 - double x_0 = 0.0; - double y_0 = 0.0; - double z_0 = 0.0; - - double Vx_0 = 0.0; - double Vy_0 = 0.0; - double Vz_0 = 0.0; - - double Ax_0 = 0.0; - double Ay_0 = 0.0; - double Az_0 = 0.0; - - double r0 = 0.0; - double r0_2 = 0.0; - double r0_3 = 0.0; - double r0_5 = 0.0; - - double dVx_1 = 0.0; - double dVy_1 = 0.0; - double dVz_1 = 0.0; - - double dx_1 = 0.0; - double dy_1 = 0.0; - double dz_1 = 0.0; - - // Runge-Kutta Integration Stage K2 - double x_1 = 0.0; - double y_1 = 0.0; - double z_1 = 0.0; - - double Vx_1 = 0.0; - double Vy_1 = 0.0; - double Vz_1 = 0.0; - - double r1 = 0.0; - double r1_2 = 0.0; - double r1_3 = 0.0; - double r1_5 = 0.0; - - double dVx_2 = 0.0; - double dVy_2 = 0.0; - double dVz_2 = 0.0; - - double dx_2 = 0.0; - double dy_2 = 0.0; - double dz_2 = 0.0; - - // Runge-Kutta Integration Stage K3 - double x_2 = 0.0; - double y_2 = 0.0; - double z_2 = 0.0; - - double Vx_2 = 0.0; - double Vy_2 = 0.0; - double Vz_2 = 0.0; - - double r2 = 0.0; - double r2_2 = 0.0; - double r2_3 = 0.0; - double r2_5 = 0.0; - - double dVx_3 = 0.0; - double dVy_3 = 0.0; - double dVz_3 = 0.0; - - double dx_3 = 0.0; - double dy_3 = 0.0; - double dz_3 = 0.0; - - // Runge-Kutta Integration Stage K4 - double x_3 = 0.0; - double y_3 = 0.0; - double z_3 = 0.0; - - double Vx_3 = 0.0; - double Vy_3 = 0.0; - double Vz_3 = 0.0; - - double r3 = 0.0; - double r3_2 = 0.0; - double r3_3 = 0.0; - double r3_5 = 0.0; - - double dVx_4 = 0.0; - double dVy_4 = 0.0; - double dVz_4 = 0.0; - - double dx_4 = 0.0; - double dy_4 = 0.0; - double dz_4 = 0.0; - - // Find time difference - dt = check_t(transmitTime - d_t_b); - - // Calculate clock correction - d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt); - - // Find integration time - tk = dt - d_satClkDrift; - - // Check if to integrate forward or backward - if (tk < 0) - { - tau = -60; - } - else - { - tau = 60; - } - - // Coordinates transformation to an inertial reference frame - // x,y,z coordinates to meters - x_0 = d_Xn * 1e3; - y_0 = d_Yn * 1e3; - z_0 = d_Zn * 1e3; - - // x,y,z velocities to meters/s - Vx_0 = d_VXn * 1e3; - Vy_0 = d_VYn * 1e3; - Vz_0 = d_VZn * 1e3; - - // x,y,z accelerations to meters/sec^2 - Ax_0 = d_AXn * 1e3; - Ay_0 = d_AYn * 1e3; - Az_0 = d_AZn * 1e3; - - for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) - { - // Check if last integration step. If last integration step, make one more step that has the remaining time length... - if(fabs(numberOfIntegrations) > fabs(tk)) - { - // if there is more time left to integrate... - if (fmod(tk,tau) != 0) - { - tau = fmod(tk,tau); - } - else // otherwise make a zero step. - { - tau = 0; - } - } - - // Runge-Kutta Integration Stage K1 - r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0); - r0_2 = r0*r0; - r0_3 = r0*r0*r0; - r0_5 = r0*r0*r0*r0*r0; - - dx_1 = Vx_0; - dy_1 = Vy_0; - dz_1 = Vz_0; - - dVx_1 = - GLONASS_GM / r0_3 * x_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * x_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jx_moon + d_Jx_sun; - dVy_1 = - GLONASS_GM / r0_3 * y_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * y_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jy_moon + d_Jy_sun; - dVz_1 = - GLONASS_GM / r0_3 * z_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * z_0 * (3 - 5 * 1 / (z_0*z_0)) + d_Jz_moon + d_Jz_sun; - - dx_1 = Vx_0; - dy_1 = Vy_0; - dz_1 = Vz_0; - - // Runge-Kutta Integration Stage K2 - Vx_1 = Vx_0 + 1/2 * tau * dVx_1; - Vy_1 = Vy_0 + 1/2 * tau * dVy_1; - Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - - x_1 = x_0 + 1/2 * tau * dx_1; - y_1 = y_0 + 1/2 * tau * dy_1; - z_1 = z_0 + 1/2 * tau * dz_1; - - r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); - r1_2 = r1*r1; - r1_3 = r1*r1*r1; - r1_5 = r1*r1*r1*r1*r1; - - dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0; - dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0; - dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; - - dx_2 = Vx_1; - dy_2 = Vy_1; - dz_2 = Vz_1; - - // Runge-Kutta Integration Stage K2 - Vx_2 = Vx_0 + 1/2 * tau * dVx_2; - Vy_2 = Vy_0 + 1/2 * tau * dVy_2; - Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - - x_2 = x_0 + 1/2 * tau * dx_2; - y_2 = y_0 + 1/2 * tau * dy_2; - z_2 = z_0 + 1/2 * tau * dz_2; - - r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); - r2_2 = r2*r2; - r2_3 = r2*r2*r2; - r2_5 = r2*r2*r2*r2*r2; - - - dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0; - dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0; - dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; - - dx_3 = Vx_2; - dy_3 = Vy_2; - dz_3 = Vz_2; - - // Runge-Kutta Integration Stage K3 - Vx_3 = Vx_0 + tau * dVx_3; - Vy_3 = Vy_0 + tau * dVy_3; - Vz_3 = Vz_0 + tau * dVz_3; - - x_3 = x_0 + tau * dx_3; - y_3 = y_0 + tau * dy_3; - z_3 = z_0 + tau * dz_3; - - r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); - r3_2 = r3*r3; - r3_3 = r3*r3*r3; - r3_5 = r3*r3*r3*r3*r3; - - dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0; - dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0; - dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; - - dx_4 = Vx_3; - dy_4 = Vy_3; - dz_4 = Vz_3; - - // Final results showcased here - d_satvel_X = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); - d_satvel_Y = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); - d_satvel_Z = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); - - d_satpos_X = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); - d_satpos_Y = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); - d_satpos_Z = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); - - } - - // Time from ephemeris reference clock - //tk = check_t(transmitTime - d_Toc); - - //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; - - /* relativity correction */ - //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); - - return d_dtr; -} diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 8a978f816..ef4a0ec12 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -60,16 +60,6 @@ private: */ double check_t(double time); - void gravitational_perturbations(); - - double d_Jx_moon; //!< Moon gravitational perturbation - double d_Jy_moon; //!< Moon gravitational perturbation - double d_Jz_moon; //!< Moon gravitational perturbation - - double d_Jx_sun; //!< Sun gravitational perturbation - double d_Jy_sun; //!< Sun gravitational perturbation - double d_Jz_sun; //!< Sun gravitational perturbation - public: 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] @@ -115,19 +105,6 @@ public: double d_TOW; // tow of the start of frame double d_WN; // week number of the start of frame - // satellite positions after RK4 Integration - double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity after RK4 Integration - double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration after RK4 Integration - double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] - template /*! @@ -171,37 +148,12 @@ 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 Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Algorithm A.3.1.2 in GLONASS ICD v5.1 - * and compute the clock bias term including relativistic effect (return value) - * \param transmitTime Time of ephemeris transmission - * \return clock bias of satellite - */ - double simplified_satellite_position(double transmitTime); - - /*! - * \brief Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Algorithm A.3.1.1 in GLONASS ICD v5.1 - * and compute the clock bias term including relativistic effect (return value) - * \param transmitTime Time of ephemeris transmission - * \return clock bias of satellite - */ - double satellite_position(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, double timeCorrUTC); - /*! - * \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); - - /*! * \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 diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index fa0886418..9e84762d6 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -1,7 +1,9 @@ /*! * \file glonass_gnav_utc_model.h * \brief Interface of a GLONASS GNAV UTC MODEL storage + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * From bf9345ebd21c074e27a84d37032089daa448adee Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 29 Aug 2017 00:16:07 -0600 Subject: [PATCH 38/48] Fixes error after rebase operation from upstream/next --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 2 +- src/algorithms/PVT/libs/rtklib_solver.cc | 270 +++++++++--------- src/algorithms/PVT/libs/rtklib_solver.h | 8 - 3 files changed, 137 insertions(+), 143 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 1ebf824f7..083aee259 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -717,7 +717,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; - std::map::iterator glonass_gnav_ephemeris_iter; + std::map::const_iterator glonass_gnav_ephemeris_iter; std::map::const_iterator gnss_observables_iter; if (!b_rinex_header_written) // & we have utc data in nav message! diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index b803a6c45..c19dd7f4b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -134,74 +134,77 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ { switch(gnss_observables_iter->second.System) { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) + case 'E': { - std::string sig_(gnss_observables_iter->second.Signal); - // Galileo E1 - if(sig_.compare("1B") == 0) - { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) - { - //convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); - //convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; - obs_data[valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN_5, - 0); - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) + { + std::string sig_(gnss_observables_iter->second.Signal); + // Galileo E1 + if(sig_.compare("1B") == 0) + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 0); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } - } - // Galileo E5 - if(sig_.compare("5X") == 0) - { + } + // Galileo E5 + if(sig_.compare("5X") == 0) + { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) - { - bool found_E1_obs=false; - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) - { - obs_data[i] = insert_obs_to_rtklib(obs_data[i], - gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN_5, - 2);//Band 3 (L5/E5) - found_E1_obs=true; - break; - } - } - if (!found_E1_obs) - { - //insert Galileo E5 obs as new obs and also insert its ephemeris - //convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); - //convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; - obs_data[valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN_5, - 2); //Band 3 (L5/E5) - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + { + bool found_E1_obs=false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) + { + obs_data[i] = insert_obs_to_rtklib(obs_data[i], + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 2);//Band 3 (L5/E5) + found_E1_obs=true; + break; + } + } + if (!found_E1_obs) + { + //insert Galileo E5 obs as new obs and also insert its ephemeris + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 2); //Band 3 (L5/E5) + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + } break; } case 'G': @@ -276,75 +279,75 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ break; } case 'R': //TODO This should be using rtk lib nomenclature - { - std::string sig_(gnss_observables_iter->second.Signal); - // GLONASS GNAV L1 - if(sig_.compare("1C") == 0) - { - // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key - glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) - { - //convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); - //convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; - obs_data[valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - glonass_gnav_ephemeris_iter->second.WN_5, - 0);//TODO are THESE VALUES OK - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } + { + std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if(sig_.compare("1C") == 0) + { + // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + geph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + 0);//TODO are THESE VALUES OK + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } - } - // GLONASS GNAV L2 - if(sig_.compare("2C") == 0) - { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) - { - bool found_L1_obs=false; - for (int i = 0; i < valid_obs; i++) - { - // TODO what is this? - if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN+NSATGPS+NSATGLO))) - { - obs_data[i] = insert_obs_to_rtklib(obs_data[i], - gnss_observables_iter->second, - glonass_gnav_ephemeris_iter->second.WN_5, - 2);//Band 3 (L5/E5) - found_L1_obs=true; - break; - } - } - if (!found_L1_obs) - { - //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris - //convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); - //convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; - obs_data[valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN_5, - 2); //Band 3 (L5/E5) - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } + } + // GLONASS GNAV L2 + if(sig_.compare("2C") == 0) + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) + { + bool found_L1_obs=false; + for (int i = 0; i < valid_obs; i++) + { + // TODO what is this? + if (geph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN+NSATGPS+NSATGLO))) + { + obs_data[i] = insert_obs_to_rtklib(obs_data[i], + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + 2);//Band 3 (L5/E5) + found_L1_obs=true; + break; + } + } + if (!found_L1_obs) + { + //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris + //convert ephemeris from GNSS-SDR class to RTKLIB structure + geph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 2); //Band 3 (L5/E5) + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } - } - break; - } + } + break; + } default : DLOG(INFO) << "Hybrid observables: Unknown GNSS"; break; @@ -439,8 +442,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); } } - } + } } - } - return b_valid_position; + return this->is_valid_position(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 5e74aa357..40e0ab250 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -103,14 +103,6 @@ public: Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model int count_valid_position; - - bool d_flag_dump_enabled; - - sol_t pvt_sol; - rtk_t rtk_; - - std::string d_dump_filename; - std::ofstream d_dump_file; }; #endif From 19f4da0aa64b7c1595ae260cc319c68a26dd2ac6 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 29 Aug 2017 01:20:38 -0600 Subject: [PATCH 39/48] merging: Starts merge procedure for glonass branch from upstream Starts merging process of the acquisition and tracking code for GLONASS processing developed by Gabriel Araujo. Fixes compile warnings and errors introduced during rebase operation --- .../glonass_l1_ca_pcps_acquisition.cc | 5 +- .../gnuradio_blocks/pcps_acquisition_cc.cc | 8 +-- .../gnuradio_blocks/pcps_acquisition_cc.h | 2 +- .../gnuradio_blocks/pcps_acquisition_sc.cc | 2 +- .../adapters/signal_generator.cc | 2 +- .../gnuradio_blocks/signal_generator_c.cc | 2 +- .../gnuradio_blocks/CMakeLists.txt | 7 +- .../glonass_l1_ca_telemetry_decoder_cc.cc | 5 +- .../glonass_l1_ca_dll_pll_c_aid_tracking.cc | 2 +- .../glonass_l1_ca_dll_pll_tracking.cc | 2 +- ...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 +- src/core/receiver/gnss_block_factory.cc | 14 ++-- src/core/system_parameters/GLONASS_L1_CA.h | 32 +++++++++ src/core/system_parameters/Glonass_L1_CA.h | 69 ------------------- 16 files changed, 60 insertions(+), 98 deletions(-) delete mode 100644 src/core/system_parameters/Glonass_L1_CA.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 5d81dc992..4d321b220 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -2,7 +2,7 @@ #include #include #include "glonass_l1_signal_processing.h" -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" #include "configuration_interface.h" @@ -63,7 +63,7 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; - + if (item_type_.compare("cbyte") == 0) { cbyte_to_float_x2_ = make_complex_byte_to_float_x2(); @@ -357,4 +357,3 @@ gr::basic_block_sptr GlonassL1CaPcpsAcquisition::get_right_block() return acquisition_cc_; } } - diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index 29506fbeb..cd314a342 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -42,7 +42,7 @@ #include #include "control_message_factory.h" #include "GPS_L1_CA.h" //GPS_TWO_PI -#include "Glonass_L1_CA.h" //GLONASS_TWO_PI +#include "GLONASS_L1_CA.h" //GLONASS_TWO_PI using google::LogMessage; @@ -178,12 +178,12 @@ void pcps_acquisition_cc::set_local_code(std::complex * code) int offset = d_fft_size/2; std::fill_n( d_fft_if->get_inbuf(), offset, gr_complex( 0.0, 0.0 ) ); memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * offset); - } - else + } + else { memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size); } - + 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); } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 4bd5ab857..272a0f9bc 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -56,7 +56,7 @@ #include #include #include "gnss_synchro.h" -#include "Glonass_L1_CA.h" //GLONASS_TWO_PI +#include "GLONASS_L1_CA.h" //GLONASS_TWO_PI class pcps_acquisition_cc; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc index 7200ce946..5a15e2037 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc @@ -41,7 +41,7 @@ #include #include "control_message_factory.h" #include "GPS_L1_CA.h" //GPS_TWO_PI -#include "Glonass_L1_CA.h" //GLONASS_TWO_PI +#include "GLONASS_L1_CA.h" //GLONASS_TWO_PI using google::LogMessage; diff --git a/src/algorithms/signal_generator/adapters/signal_generator.cc b/src/algorithms/signal_generator/adapters/signal_generator.cc index a2ef90cdf..57e3c0aa0 100644 --- a/src/algorithms/signal_generator/adapters/signal_generator.cc +++ b/src/algorithms/signal_generator/adapters/signal_generator.cc @@ -36,7 +36,7 @@ #include "Galileo_E1.h" #include "GPS_L1_CA.h" #include "Galileo_E5a.h" -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" using google::LogMessage; 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 a99c4751d..2e383f40b 100644 --- a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc +++ b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc @@ -40,7 +40,7 @@ #include "Galileo_E1.h" #include "Galileo_E5a.h" #include "GPS_L1_CA.h" -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" /* * Create a new instance of signal_generator_c and return diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index 7de1a365b..043440927 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -16,14 +16,15 @@ # along with GNSS-SDR. If not, see . # -set(TELEMETRY_DECODER_GR_BLOCKS_SOURCES +set(TELEMETRY_DECODER_GR_BLOCKS_SOURCES gps_l1_ca_telemetry_decoder_cc.cc gps_l2c_telemetry_decoder_cc.cc galileo_e1b_telemetry_decoder_cc.cc sbas_l1_telemetry_decoder_cc.cc galileo_e5a_telemetry_decoder_cc.cc + glonass_l1_ca_telemetry_decoder_cc.cc ) - + include_directories( ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_SOURCE_DIR}/src/core/system_parameters @@ -40,4 +41,4 @@ file(GLOB TELEMETRY_DECODER_GR_BLOCKS_HEADERS "*.h") list(SORT TELEMETRY_DECODER_GR_BLOCKS_HEADERS) add_library(telemetry_decoder_gr_blocks ${TELEMETRY_DECODER_GR_BLOCKS_SOURCES} ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS}) source_group(Headers FILES ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS}) -target_link_libraries(telemetry_decoder_gr_blocks telemetry_decoder_libswiftcnav telemetry_decoder_lib gnss_system_parameters ${GNURADIO_RUNTIME_LIBRARIES}) \ No newline at end of file +target_link_libraries(telemetry_decoder_gr_blocks telemetry_decoder_libswiftcnav telemetry_decoder_lib gnss_system_parameters ${GNURADIO_RUNTIME_LIBRARIES}) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index 3d63c38ea..6a9bc0944 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -71,7 +71,7 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY PROCESSING"; // Define the number of sampes per symbol. Notice that GLONASS has to rates, //one for the navigation data and the other for the preamble information - d_samples_per_symbol = ( GLONASS_L1_CODE_CHIP_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS ) / GLONASS_L1_CA_SYMBOL_RATE_BPS; + d_samples_per_symbol = ( GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS ) / GLONASS_L1_CA_SYMBOL_RATE_BPS; // Set the preamble information unsigned short int preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS] = GLONASS_GNAV_PREAMBLE; @@ -108,7 +108,6 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( d_TOW_at_current_symbol = 0; delta_t = 0; d_CRC_error_counter = 0; - flag_even_word_arrived = 0; d_flag_preamble = false; d_channel = 0; flag_TOW_set = false; @@ -149,7 +148,7 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int } // 2. Call the GLONASS GNAV string decoder - d_nav.decode_string(page_String); + d_nav.string_decoder(frame_string); // 3. Check operation executed correctly if(d_nav.flag_CRC_test == true) diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc index 4f73fa200..8ae399d7f 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc @@ -1,6 +1,6 @@ #include "glonass_l1_ca_dll_pll_c_aid_tracking.h" #include -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" #include "configuration_interface.h" diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc index cce1b19a7..da7fd324c 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc @@ -38,7 +38,7 @@ #include "glonass_l1_ca_dll_pll_tracking.h" #include -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" #include "configuration_interface.h" 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 f08edc5a3..9b9818717 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 @@ -12,7 +12,7 @@ #include "glonass_l1_signal_processing.h" #include "tracking_discriminators.h" #include "lock_detectors.h" -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" #include "control_message_factory.h" 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 e13c48108..dcd94d281 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 @@ -13,7 +13,7 @@ #include "glonass_l1_signal_processing.h" #include "tracking_discriminators.h" #include "lock_detectors.h" -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" #include "control_message_factory.h" 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 65e756c2e..052d89a36 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 @@ -10,7 +10,7 @@ #include "glonass_l1_signal_processing.h" #include "tracking_discriminators.h" #include "lock_detectors.h" -#include "Glonass_L1_CA.h" +#include "GLONASS_L1_CA.h" #include "control_message_factory.h" diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 1f9c73529..927aaf0fe 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -529,7 +529,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_1G( stream << channel; std::string id = stream.str(); LOG(INFO) << "Instantiating Channel " << channel << " with Acquisition Implementation: " - << acq << ", Tracking Implementation: " << trk << ", Telemetry Decoder implementation: " << tlm; + << acq << ", Tracking Implementation: " << trk << ", Telemetry Decoder Implementation: " << tlm; std::string aux = configuration->property("Acquisition_1G" + boost::lexical_cast(channel) + ".implementation", std::string("W")); std::string appendix1; @@ -1354,12 +1354,6 @@ std::unique_ptr GNSSBlockFactory::GetAcqBlock( out_streams)); block = std::move(block_); } - else if (implementation.compare("GLONASS_L1_CA_Telemetry_Decoder") == 0) - { - std::unique_ptr block_(new GlonassL1CaTelemetryDecoder(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } else { // Log fatal. This causes execution to stop. @@ -1496,6 +1490,12 @@ std::unique_ptr GNSSBlockFactory::GetTlmBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GLONASS_L1_CA_Telemetry_Decoder") == 0) + { + std::unique_ptr block_(new GlonassL1CaTelemetryDecoder(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else { // Log fatal. This causes execution to stop. diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 3b03a58e5..750b41136 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -88,6 +88,38 @@ const double GLONASS_L1_CA_CODE_PERIOD = 0.001; //!< GLONASS L1 C/ const double GLONASS_L1_CA_CHIP_PERIOD = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds] const double GLONASS_L1_CA_SYMBOL_RATE_BPS = 1000; +// GLONASS SV's orbital slots PRN = (orbital_slot - 1) +const std::map GLONASS_PRN = + {{ 0, 8,}, //For test + { 1, 1,}, //Plane 1 + { 2,-4,}, //Plane 1 + { 3, 5,}, //Plane 1 + { 4, 6,}, //Plane 1 + { 5, 1,}, //Plane 1 + { 6,-4,}, //Plane 1 + { 7, 5,}, //Plane 1 + { 8, 6,}, //Plane 1 + { 9,-2,}, //Plane 2 + {10,-7,}, //Plane 2 + {11, 0,}, //Plane 2 + {12,-1,}, //Plane 2 + {13,-2,}, //Plane 2 + {14,-7,}, //Plane 2 + {15, 0,}, //Plane 2 + {16,-1,}, //Plane 2 + {17, 4,}, //Plane 3 + {18,-3,}, //Plane 3 + {19, 3,}, //Plane 3 + {20, 2,}, //Plane 3 + {21, 4,}, //Plane 3 + {22,-3,}, //Plane 3 + {23, 3,}, //Plane 3 + {24, 2}}; //Plane 3 + + +const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] + + const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) // OBSERVABLE HISTORY DEEP FOR INTERPOLATION diff --git a/src/core/system_parameters/Glonass_L1_CA.h b/src/core/system_parameters/Glonass_L1_CA.h deleted file mode 100644 index bcdfd2b32..000000000 --- a/src/core/system_parameters/Glonass_L1_CA.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef GNSS_SDR_GLONASS_L1_CA_H_ -#define GNSS_SDR_GLONASS_L1_CA_H_ - -#include -#include // std::map -#include "MATH_CONSTANTS.h" -#include "gnss_frequencies.h" - -// Physical constants -const double GLONASS_C_m_s = 299792458.0; //!< The speed of light, [m/s] -const double GLONASS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] -const double GLONASS_PI = 3.1415926535898; //!< Pi as (NOT) defined in ICD-GLONASS-2008 -const double GLONASS_TWO_PI = 6.283185307179586;//!< 2Pi as (NOT) defined in ICD-GLONASS-2008 -const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s] -const double GLONASS_GM = 3.986004418e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] -// const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] - -// Geodesic constants and parameters -const double fMa = 0.35e9; //!< The Gravitational constant of atmosphere, [m^3/s^2] -const double SEMI_MAJOR_AXIS = 6378136; //!< The Semi-major axis, [m] -const double FLATTENING = 1/298.25784; //!< The Orbital Flattening -const double EQUATORIAL_GRAVITY = 978032.84; //!< The Equatorial acceleration of gravity, [mGal] -const double GRAVITY_CORRECTION = 0.87; //!< The Correction to acceleration of gravity at sea-level due to Atmosphere, [mGal] -const double SECOND_HARMONIC = 1082625.75e-9; //!< Second zonal harmonic of the geopotential (J_2^0) -const double FOURTH_HARMONIC = -2370.89e-9; //!< Fourth zonal harmonic of the geopotential (J_4^0) -const double SIXTH_HARMONIC = 6.08e-9; //!< Sixth zonal harmonic of the geopotential (J_6^0) -const double EIGHTH_HARMONIC = 1.40e-11; //!< Eighth zonal harmonic of the geopotential (J_8^0) -const double NORMAL_POTENCIAL = 62636861.4; //!< The Normal potential at surface of common terrestrial ellipsoid (U_0), [m^2/s^2] - - -// carrier and code frequencies -const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] -const double GLONASS_L1_CA_CODE_RATE_HZ = 0.511e6; //!< GLONASS L1 C/A code rate [chips/s] -const double GLONASS_L1_CA_CODE_LENGTH_CHIPS = 511.0; //!< GLONASS L1 C/A code length [chips] -const double GLONASS_L1_CA_CODE_PERIOD = 0.001; //!< GLONASS L1 C/A code period [seconds] -const double GLONASS_L1_CA_CHIP_PERIOD = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds] - -// GLONASS SV's orbital slots PRN = (orbital_slot - 1) -const std::map GLONASS_PRN = - {{ 0, 8,}, //For test - { 1, 1,}, //Plane 1 - { 2,-4,}, //Plane 1 - { 3, 5,}, //Plane 1 - { 4, 6,}, //Plane 1 - { 5, 1,}, //Plane 1 - { 6,-4,}, //Plane 1 - { 7, 5,}, //Plane 1 - { 8, 6,}, //Plane 1 - { 9,-2,}, //Plane 2 - {10,-7,}, //Plane 2 - {11, 0,}, //Plane 2 - {12,-1,}, //Plane 2 - {13,-2,}, //Plane 2 - {14,-7,}, //Plane 2 - {15, 0,}, //Plane 2 - {16,-1,}, //Plane 2 - {17, 4,}, //Plane 3 - {18,-3,}, //Plane 3 - {19, 3,}, //Plane 3 - {20, 2,}, //Plane 3 - {21, 4,}, //Plane 3 - {22,-3,}, //Plane 3 - {23, 3,}, //Plane 3 - {24, 2}}; //Plane 3 - - -const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] - -#endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ From 4e78bcf446988b37961e0113785dce34613cedb9 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Tue, 29 Aug 2017 17:32:34 -0600 Subject: [PATCH 40/48] Merge in GLONASS acquisition and tracking code --- conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf | 81 +++++++++++++++++ .../gnuradio_blocks/signal_generator_c.cc | 2 +- .../telemetry_decoder/adapters/CMakeLists.txt | 7 +- .../glonass_l1_ca_telemetry_decoder_cc.cc | 90 +++++++++++-------- ...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 +- src/core/system_parameters/GLONASS_L1_CA.h | 20 ++--- .../glonass_gnav_navigation_message.cc | 29 +++--- .../glonass_gnav_navigation_message.h | 22 +++-- src/core/system_parameters/rtcm.cc | 8 +- .../glonass_gnav_nav_message_test.cc | 10 +-- 12 files changed, 179 insertions(+), 96 deletions(-) create mode 100644 conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf diff --git a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf new file mode 100644 index 000000000..83dbf6fcb --- /dev/null +++ b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf @@ -0,0 +1,81 @@ +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_hz=6625000 + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/home/gastd/sdr/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_schar_1m.bin +;SignalSource.filename=/home/gastd/sdr/signal_glonass.bin +SignalSource.item_type=ibyte +SignalSource.sampling_frequency=6625000 +;SignalSource.freq=0 +;SignalSource.samples=66250000 +SignalSource.samples=0 +SignalSource.dump=false; +SignalSource.dump_filename=./signal_glonass.bin + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner +DataTypeAdapter.implementation=Ibyte_To_Complex +InputFilter.implementation=Pass_Through +InputFilter.item_type=gr_complex +Resampler.implementation=Direct_Resampler +Resampler.sample_freq_in=6625000 +Resampler.sample_freq_out=6625000 +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1G.count=2 +Channels.in_acquisition=1 +Channel0.signal=1G +Channel0.signal=1G +Channel1.signal=1G +Channel2.signal=1G +Channel3.signal=1G +Channel4.signal=1G +Channel5.signal=1G +Channel6.signal=1G +Channel7.signal=1G + +Channel0.satellite=2 +Channel1.satellite=21 +Channel2.satellite=12 +;Channel3.satellite=2 +;Channel4.satellite=22 +;Channel5.satellite=21 + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_1G.implementation=GLONASS_L1_CA_PCPS_Acquisition +Acquisition_1G.item_type=gr_complex +Acquisition_1G.threshold=0.0 +Acquisition_1G.pfa=0.01 +Acquisition_1G.if=0 +Acquisition_1G.doppler_max=10000 +Acquisition_1G.doppler_step=250 +Acquisition_1G.dump=false; +Acquisition_1G.dump_filename=./acquisition.dat + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_1G.implementation=GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking +Tracking_1G.item_type=gr_complex +Tracking_1G.if=0 +Tracking_1G.early_late_space_chips=0.5 +Tracking_1G.pll_bw_hz=20.0; +Tracking_1G.dll_bw_hz=4.0; +Tracking_1G.dump=false; +Tracking_1G.dump_filename=./tracking_ch_ + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1G.implementation=GPS_L1_CA_Telemetry_Decoder + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=Hybrid_Observables + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.averaging_depth=100 +PVT.flag_averaging=true +PVT.output_rate_ms=10 +PVT.display_rate_ms=500 + 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 2e383f40b..6d84c3084 100644 --- a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc +++ b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc @@ -115,7 +115,7 @@ void signal_generator_c::init() / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS))); num_of_codes_per_vector_.push_back(galileo_signal ? 4 * static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH) : 1); - data_bit_duration_ms_.push_back(1e3 / GLONASS_CA_TELEMETRY_RATE_BITS_SECOND); + data_bit_duration_ms_.push_back(1e3 / GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND); } else if (system_[sat] == "E") { diff --git a/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt b/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt index 974adfb53..41e9bffd1 100644 --- a/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt @@ -16,12 +16,13 @@ # along with GNSS-SDR. If not, see . # -set(TELEMETRY_DECODER_ADAPTER_SOURCES - gps_l1_ca_telemetry_decoder.cc - gps_l2c_telemetry_decoder.cc +set(TELEMETRY_DECODER_ADAPTER_SOURCES + gps_l1_ca_telemetry_decoder.cc + gps_l2c_telemetry_decoder.cc galileo_e1b_telemetry_decoder.cc sbas_l1_telemetry_decoder.cc galileo_e5a_telemetry_decoder.cc + glonass_l1_ca_telemetry_decoder.cc ) include_directories( diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index 6a9bc0944..21c549b88 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -58,8 +58,8 @@ glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump) glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( Gnss_Satellite satellite, bool dump) : - gr::block("glonass_l1_ca_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + gr::block("glonass_l1_ca_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); @@ -68,8 +68,8 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY PROCESSING"; - // Define the number of sampes per symbol. Notice that GLONASS has to rates, + LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY DECODING"; + // Define the number of sampes per symbol. Notice that GLONASS has 2 rates, //one for the navigation data and the other for the preamble information d_samples_per_symbol = ( GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS ) / GLONASS_L1_CA_SYMBOL_RATE_BPS; @@ -83,9 +83,9 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc( // preamble bits to sampled symbols d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble); int n = 0; - for (int i = 0; i < GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT; i++) + for (int i = 0; i < GLONASS_GNAV_PREAMBLE_LENGTH_BITS; i++) { - for (unsigned int j = 0; j < d_samples_per_symbol; j++) + for (unsigned int j = 0; j < GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT; j++) { if (d_preambles_bits[i] == 1) { @@ -133,22 +133,45 @@ glonass_l1_ca_telemetry_decoder_cc::~glonass_l1_ca_telemetry_decoder_cc() void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int frame_length) { + double chip_acc = 0.0; + int chip_acc_counter = 0; + // 1. Transform from symbols to bits - std::string frame_string; + std::string bi_binary_code; + std::string relative_code; + // Group samples into bi-binary code for(int i = 0; i < (frame_length); i++) { - if (frame_symbols[i] > 0) - { - frame_string.push_back('1'); - } + chip_acc += frame_symbols[i]; + chip_acc_counter += 1; + + if(chip_acc_counter == (GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT - 1)) + { + if (chip_acc > 0) + { + bi_binary_code.push_back('1'); + } + else + { + bi_binary_code.push_back('0'); + } + } + } + // Convert from bi-binary code to relative code + for(int i = 0; i < (GLONASS_GNAV_STRING_BITS); i++) + { + if(bi_binary_code[2*i] == '1' && bi_binary_code[2*i + 1] == '0') + { + relative_code.push_back('1'); + } else - { - frame_string.push_back('0'); - } + { + relative_code.push_back('0'); + } } // 2. Call the GLONASS GNAV string decoder - d_nav.string_decoder(frame_string); + d_nav.string_decoder(relative_code.c_str()); // 3. Check operation executed correctly if(d_nav.flag_CRC_test == true) @@ -178,7 +201,8 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int } if (d_nav.have_new_almanac() == true) { - std::shared_ptr tmp_obj= std::make_shared(d_nav.get_almanac()); + unsigned int slot_nbr = d_nav.get_ephemeris().i_satellite_slot_number; + 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)); } } @@ -201,7 +225,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib consume_each(1); d_flag_preamble = false; - unsigned int required_symbols=GLONASS_GNAV_FRAME_BITS+d_symbols_per_preamble; + unsigned int required_symbols=GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS; if (d_symbol_history.size()>required_symbols) { @@ -224,9 +248,11 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib { if (abs(corr_value) >= d_symbols_per_preamble) { - d_preamble_index = d_sample_counter;//record the preamble sample stamp + // Record the preamble sample stamp + d_preamble_index = d_sample_counter; LOG(INFO) << "Preamble detection for GLONASS L1 C/A SAT " << this->d_satellite; - d_stat = 1; // enter into frame pre-detection status + // Enter into frame pre-detection status + d_stat = 1; } } else if (d_stat == 1) // posible preamble lock @@ -253,39 +279,29 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib } else if (d_stat == 2) { - if (d_sample_counter == d_preamble_index + GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) + if (d_sample_counter == d_preamble_index + GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS) { // NEW GLONASS string received // 0. fetch the symbols into an array - int frame_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; - double frame_symbols[frame_length]; + int string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; + double string_symbols[string_length]; //******* SYMBOL TO BIT ******* - if (d_symbol_history.at(0).Flag_valid_symbol_output == true) - { - // extended correlation to bit period is enabled in tracking! - d_symbol_accumulator += d_symbol_history.at(0).Prompt_I; // accumulate the input value in d_symbol_accumulator - d_symbol_accumulator_counter += d_symbol_history.at(0).correlation_length_ms; - } - if (d_symbol_accumulator_counter >= 20) - { - } - - for (int i = 0; i < frame_length; i++) + for (int i = 0; i < string_length; i++) { if (corr_value > 0) { - page_part_symbols[i] = d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now! + string_symbols[i] = d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now! } else { - page_part_symbols[i] = -d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now! + string_symbols[i] = -d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now! } } //call the decoder - decode_string(page_part_symbols); + decode_string(string_symbols, string_length); if (d_nav.flag_CRC_test == true) { d_CRC_error_counter = 0; @@ -317,7 +333,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) //update TOW at the preamble instant { - d_TOW_at_current_symbol = floor((d_nav.d_TOW + 2*GLONASS_L1_CA_CODE_PERIOD + GLONASS_CA_PREAMBLE_DURATION_S)*1000.0)/1000.0; + d_TOW_at_current_symbol = floor((d_nav.d_TOW + 2*GLONASS_L1_CA_CODE_PERIOD + GLONASS_GNAV_PREAMBLE_DURATION_S)*1000.0)/1000.0; } else //if there is not a new preamble, we define the TOW of the current symbol 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 9b9818717..42f80725e 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 @@ -196,7 +196,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; 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 dcd94d281..37b9ebaa7 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 @@ -197,7 +197,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (GLONASS_L1_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; 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 052d89a36..4bdb0abf3 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 @@ -161,7 +161,7 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 750b41136..913f42fcd 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -80,8 +80,8 @@ const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentr // carrier and code frequencies const double GLONASS_L2_FREQ_HZ = FREQ2_GLO; //!< L1 [Hz] -const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] -const double GLONASS_L1_DFREQ_HZ = DFRQ1_GLO; //!< Freq Bias for GLONASS L1 [Hz] +const double GLONASS_L1_CA_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] +const double GLONASS_L1_CA_DFREQ_HZ = DFRQ1_GLO; //!< Freq Bias for GLONASS L1 [Hz] const double GLONASS_L1_CA_CODE_RATE_HZ = 0.511e6; //!< GLONASS L1 C/A code rate [chips/s] const double GLONASS_L1_CA_CODE_LENGTH_CHIPS = 511.0; //!< GLONASS L1 C/A code length [chips] const double GLONASS_L1_CA_CODE_PERIOD = 0.001; //!< GLONASS L1 C/A code period [seconds] @@ -116,10 +116,6 @@ const std::map GLONASS_PRN = {23, 3,}, //Plane 3 {24, 2}}; //Plane 3 - -const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] - - const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) // OBSERVABLE HISTORY DEEP FOR INTERPOLATION @@ -127,19 +123,17 @@ const int GLONASS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING #define GLONASS_GNAV_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0} +const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3; const int GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30; const int GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300; -const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3; +const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; const int GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10; const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10; const int GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND*GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s] -const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; -const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame in the GNAV message 15 strings*(85 data bits + 30 time mark bits)[bits] -const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] -const int GLONASS_GNAV_FRAME_MS = 30000; //!< Subframe duration [seconds] -const int GLONASS_GNAV_STRING_BITS = 85; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] -const int GLONASS_GNAV_HAMMING_CODE_BITS = 8; //!< Number of bits in hamming code sequence of GNAV message +const int GLONASS_GNAV_STRING_SYMBOLS = 2000; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] +const int GLONASS_GNAV_STRING_BITS = 85; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] +const int GLONASS_GNAV_HAMMING_CODE_BITS = 8; //!< Number of bits in hamming code sequence of GNAV message const std::vector GLONASS_GNAV_CRC_I_INDEX {9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84}; const std::vector GLONASS_GNAV_CRC_J_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 0b64dc066..bd73917ce 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -37,6 +37,7 @@ #include #include #include +#include void Glonass_Gnav_Navigation_Message::reset() @@ -70,12 +71,9 @@ void Glonass_Gnav_Navigation_Message::reset() flag_utc_model_valid = false; //!< If set, it indicates that the UTC model parameters are filled flag_utc_model_str_5 = false; //!< Clock info send in string 5 of navigation data flag_utc_model_str_15 = false; //!< Clock info send in string 15 of frame 5 of navigation data - flag_TOW_5 = false; - flag_TOW_6 = false; - flag_TOW_set = false; //!< it is true when page 5 or page 6 arrives //broadcast orbit 1 - //TODO Need to send the information regarding the frame number + flag_TOW_set = false; d_TOW = false; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] d_TOW_F1 = false; //!< Time of GPS Week from HOW word of Subframe 1 [s] d_TOW_F2 = false; //!< Time of GPS Week from HOW word of Subframe 2 [s] @@ -313,14 +311,14 @@ unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int sate } -int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) +int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) { int string_ID = 0; int J = 0; frame_ID = 0; // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE - std::bitset string_bits = std::bitset(std::string(frame_string)); + std::bitset string_bits = std::bitset((frame_string)); string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); CRC_test(string_bits); @@ -401,6 +399,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_utc_model.d_tau_gps = static_cast(read_navigation_signed(string_bits, TAU_GPS)) * TWO_N30; gnav_ephemeris.d_l5th_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + flag_utc_model_str_5 = true; // Compute Year and DoY based on Algorithm A3.11 of GLONASS ICD if(flag_ephemeris_str_4 == true) { @@ -487,7 +486,6 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) flag_almanac_str_8 = true; break; - case 9: // --- It is string 9 ---------------------------------------------- if (flag_almanac_str_8 == true) @@ -666,7 +664,7 @@ Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model() } -Glonass_Gnav_Almanac Glonass_Gnav_Navigation_Message::get_almanac( int satellite_slot_number) +Glonass_Gnav_Almanac Glonass_Gnav_Navigation_Message::get_almanac(unsigned int satellite_slot_number) { return gnav_almanac[satellite_slot_number - 1]; } @@ -674,21 +672,17 @@ Glonass_Gnav_Almanac Glonass_Gnav_Navigation_Message::get_almanac( int satellite bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class { - bool flag_data_valid = false; - bool b_valid_ephemeris_set_flag = false; - if ((flag_ephemeris_str_1 == true) and (flag_ephemeris_str_2 == true) and (flag_ephemeris_str_3 == true) and (flag_ephemeris_str_4 == true)) { - //if all ephemeris pages have the same IOD, then they belong to the same block - if ((gnav_ephemeris.d_t_b == 0) ) + if ((gnav_ephemeris.d_P_4 == 1) ) { flag_ephemeris_str_1 = false;// clear the flag flag_ephemeris_str_2 = false;// clear the flag flag_ephemeris_str_3 = false;// clear the flag flag_ephemeris_str_4 = false;// clear the flag flag_all_ephemeris = true; - // std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; - // std::cout << "Batch number: "<< IOD_ephemeris << std::endl; + DLOG(INFO) << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; + return true; } else @@ -703,10 +697,9 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a bool Glonass_Gnav_Navigation_Message::have_new_utc_model() // Check if we have a new utc data set stored in the galileo navigation class { - bool flag_utc_model = true; - if (flag_utc_model == true) + if (flag_utc_model_valid == true) { - flag_utc_model = false; // clear the flag + flag_utc_model_valid = false; // clear the flag return true; } else diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 88468e380..3cb73c819 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -63,17 +63,17 @@ public: bool flag_CRC_test; unsigned int frame_ID; - Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded - Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information - Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites + Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded + Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information + Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites //!< Satellite Identification - int i_channel_ID; //!< Channel ID assigned by the receiver - unsigned int i_satellite_freq_channel; //!< SV Frequency Slot Number - unsigned int i_satellite_slot_number; //!< SV Orbit Slot Number + int i_channel_ID; //!< Channel ID assigned by the receiver + unsigned int i_satellite_freq_channel; //!< SV Frequency Slot Number + unsigned int i_satellite_slot_number; //!< SV Orbit Slot Number //!< Ephmeris Flags - bool flag_all_ephemeris; //!< Flag indicating that all strings containing ephemeris have been received + bool flag_all_ephemeris; //!< Flag indicating that all strings containing ephemeris have been received bool flag_ephemeris_str_1; //!< Flag indicating that ephemeris 1/4 (string 1) have been received bool flag_ephemeris_str_2; //!< Flag indicating that ephemeris 2/4 (string 2) have been received bool flag_ephemeris_str_3; //!< Flag indicating that ephemeris 3/4 (string 3) have been received @@ -96,12 +96,10 @@ public: bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled bool flag_utc_model_str_5; //!< Clock info send in string 5 of navigation data bool flag_utc_model_str_15; //!< Clock info send in string 15 of frame 5 of navigation data - bool flag_TOW_5; - bool flag_TOW_6; - bool flag_TOW_set; //!< it is true when page 5 or page 6 arrives //broadcast orbit 1 //TODO Need to send the information regarding the frame number + bool flag_TOW_set; double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s] @@ -136,7 +134,7 @@ public: /* * \brief Returns a Galileo_Almanac object filled with the latest navigation data received */ - Glonass_Gnav_Almanac get_almanac(int satellite_slot_number); + Glonass_Gnav_Almanac get_almanac(unsigned int satellite_slot_number); /* * \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed @@ -156,7 +154,7 @@ public: /*! * \brief Decodes the GLONASS GNAV string */ - int string_decoder(char *string); + int string_decoder(std::string frame_string); /*! * \brief Computes the Coordinated Universal Time (UTC) and returns it in [s] diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index bce3da0c4..7ee09e6bb 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -3960,7 +3960,7 @@ int Rtcm::set_DF041(const Gnss_Synchro & gnss_synchro) //TODO Need to fix the lambda value since it needs to include frequency channel int Rtcm::set_DF042(const Gnss_Synchro & gnss_synchro) { - const double lambda = GLONASS_C_m_s / GLONASS_L1_FREQ_HZ; + const double lambda = GLONASS_C_m_s / GLONASS_L1_CA_FREQ_HZ; double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 599584.92 ); double glonass_L1_pseudorange = std::round(( gnss_synchro.Pseudorange_m - ambiguity * 599584.92) / 0.02 ); double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 299792.458; @@ -5272,7 +5272,7 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro) if ((sig.compare("1C") == 0) && (sys.compare("R") == 0 )) { // TODO Need to add slot number and freq number to gnss_syncro - lambda = GLONASS_C_m_s / (GLONASS_L1_FREQ_HZ); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ); } if ((sig.compare("2C") == 0) && (sys.compare("R") == 0 )) { @@ -5382,7 +5382,7 @@ int Rtcm::set_DF404(const Gnss_Synchro & gnss_synchro) if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) { //TODO Need to add slot number and freq number to gnss syncro - lambda = GLONASS_C_m_s / (GLONASS_L1_FREQ_HZ); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ); } if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) { @@ -5471,7 +5471,7 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro) if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) { //TODO Need to add slot number and freq number to gnss syncro - lambda = GLONASS_C_m_s / (GLONASS_L1_FREQ_HZ); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ); } if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) { diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc index df7d740fd..de814391f 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_nav_message_test.cc @@ -81,7 +81,7 @@ TEST(GlonassGnavNavigationMessageTest, String1Decoder) gnav_ephemeris.d_Xn = -11025.6669921875; // Call target test method - gnav_nav_message.string_decoder(const_cast (str1.c_str())); + gnav_nav_message.string_decoder(str1); // Perform assertions of decoded fields ASSERT_TRUE(gnav_ephemeris.d_P_1 - gnav_nav_message.gnav_ephemeris.d_P_1 < FLT_EPSILON ); @@ -116,7 +116,7 @@ TEST(GlonassGnavNavigationMessageTest, String2Decoder) // Call target test method gnav_nav_message.flag_ephemeris_str_1 = true; gnav_nav_message.gnav_ephemeris.d_P_1 = 15; - gnav_nav_message.string_decoder(const_cast (str2.c_str())); + gnav_nav_message.string_decoder(str2); // Perform assertions of decoded fields ASSERT_TRUE(gnav_ephemeris.d_B_n - gnav_nav_message.gnav_ephemeris.d_B_n < FLT_EPSILON ); @@ -151,7 +151,7 @@ TEST(GlonassGnavNavigationMessageTest, String3Decoder) gnav_ephemeris.d_Zn = 19929.2377929688; // Call target test method - gnav_nav_message.string_decoder(const_cast (str3.c_str())); + gnav_nav_message.string_decoder(str3); // Perform assertions of decoded fields ASSERT_TRUE(gnav_ephemeris.d_P_3 - gnav_nav_message.gnav_ephemeris.d_P_3 < FLT_EPSILON ); @@ -188,7 +188,7 @@ TEST(GlonassGnavNavigationMessageTest, String4Decoder) gnav_ephemeris.d_M = 1; // Call target test method - gnav_nav_message.string_decoder(const_cast (str4.c_str())); + gnav_nav_message.string_decoder(str4); // Perform assertions of decoded fields ASSERT_TRUE(gnav_ephemeris.d_tau_n - gnav_nav_message.gnav_ephemeris.d_tau_n < FLT_EPSILON ); @@ -222,7 +222,7 @@ TEST(GlonassGnavNavigationMessageTest, String5Decoder) gnav_utc_model.d_tau_gps = 9.313225746154785e-08; // Call target test method - gnav_nav_message.string_decoder(const_cast (str5.c_str())); + gnav_nav_message.string_decoder(str5); // Perform assertions of decoded fields ASSERT_TRUE(gnav_utc_model.d_N_A - gnav_nav_message.gnav_utc_model.d_N_A < FLT_EPSILON ); From 6331ebd8b5eb0660b3cab8ccbe18087755b744b9 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 30 Aug 2017 00:16:08 -0600 Subject: [PATCH 41/48] Fixes issues detected during merge procedure --- conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf | 19 +++---- src/algorithms/PVT/adapters/rtklib_pvt.cc | 52 ++++++++++---------- src/core/system_parameters/gnss_satellite.cc | 2 +- 3 files changed, 33 insertions(+), 40 deletions(-) diff --git a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf index 83dbf6fcb..9b7c1cfa5 100644 --- a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf +++ b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf @@ -5,8 +5,7 @@ GNSS-SDR.internal_fs_hz=6625000 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=File_Signal_Source -SignalSource.filename=/home/gastd/sdr/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_schar_1m.bin -;SignalSource.filename=/home/gastd/sdr/signal_glonass.bin +SignalSource.filename=/archive/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_schar_1m.bin SignalSource.item_type=ibyte SignalSource.sampling_frequency=6625000 ;SignalSource.freq=0 @@ -29,21 +28,16 @@ Resampler.item_type=gr_complex Channels_1G.count=2 Channels.in_acquisition=1 Channel0.signal=1G -Channel0.signal=1G Channel1.signal=1G Channel2.signal=1G Channel3.signal=1G Channel4.signal=1G -Channel5.signal=1G -Channel6.signal=1G -Channel7.signal=1G Channel0.satellite=2 Channel1.satellite=21 Channel2.satellite=12 -;Channel3.satellite=2 -;Channel4.satellite=22 -;Channel5.satellite=21 +Channel3.satellite=23 +; Possible list includes 2, 12, 21, 22 ;######### ACQUISITION GLOBAL CONFIG ############ Acquisition_1G.implementation=GLONASS_L1_CA_PCPS_Acquisition @@ -54,7 +48,7 @@ Acquisition_1G.if=0 Acquisition_1G.doppler_max=10000 Acquisition_1G.doppler_step=250 Acquisition_1G.dump=false; -Acquisition_1G.dump_filename=./acquisition.dat +Acquisition_1G.dump_filename=../data/glo_acquisition.dat ;######### TRACKING GLOBAL CONFIG ############ Tracking_1G.implementation=GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking @@ -63,8 +57,8 @@ Tracking_1G.if=0 Tracking_1G.early_late_space_chips=0.5 Tracking_1G.pll_bw_hz=20.0; Tracking_1G.dll_bw_hz=4.0; -Tracking_1G.dump=false; -Tracking_1G.dump_filename=./tracking_ch_ +Tracking_1G.dump=true; +Tracking_1G.dump_filename=../data/glo_tracking_ch_ ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1G.implementation=GPS_L1_CA_Telemetry_Decoder @@ -78,4 +72,3 @@ PVT.averaging_depth=100 PVT.flag_averaging=true PVT.output_rate_ms=10 PVT.display_rate_ms=500 - diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 757a2f9d0..2dbd59145 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -156,37 +156,37 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int gal_1B_count = configuration->property("Channels_1B.count", 0); int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? int gal_E5b_count = configuration->property("Channels_7X.count", 0); - int glo_1R_count = configuration->property("Channels_1R.count", 0); + int glo_1G_count = configuration->property("Channels_1G.count", 0); unsigned int type_of_receiver = 0; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 1; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 2; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 1; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 2; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 4; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 5; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 6; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 4; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 5; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 6; - if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 7; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 7; //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 9; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 10; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 11; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 12; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 9; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 10; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 11; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 12; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 14; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 15; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 14; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 15; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 17; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1R_count == 0)) type_of_receiver = 18; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 17; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 18; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; - if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0)) type_of_receiver = 21; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 21; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0)) type_of_receiver = 23; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count == 0) && (glo_2R_count != 0)) type_of_receiver = 24; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0) && (glo_1R_count != 0)) type_of_receiver = 25; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0)) type_of_receiver = 26; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1R_count != 0)) type_of_receiver = 27; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 23; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2R_count != 0)) type_of_receiver = 24; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0) && (glo_1G_count != 0)) type_of_receiver = 25; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 26; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 27; //RTKLIB PVT solver options // Settings 1 int positioning_mode = -1; @@ -209,10 +209,10 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, } int num_bands = 0; - if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) num_bands = 1; - if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; - if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0)) ) num_bands = 2; - if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1R_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) num_bands = 3; + if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) num_bands = 1; + if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; + if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0)) ) num_bands = 2; + if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) num_bands = 3; int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ if( (number_of_frequencies < 1) || (number_of_frequencies > 3) ) { @@ -294,7 +294,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int nsys = 0; if ((gps_1C_count > 0) || (gps_2S_count > 0)) nsys += SYS_GPS; if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL; - if ((glo_1R_count > 0)) nsys += SYS_GLO; + if ((glo_1G_count > 0)) nsys += SYS_GLO; int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ if( (navigation_system < 1) || (navigation_system > 255) ) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ { diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 6e0a6e4d8..a64ba740d 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -157,7 +157,7 @@ void Gnss_Satellite::set_PRN(unsigned int PRN_) PRN = PRN_; } } - else if (system.compare("GLONASS") == 0) + else if (system.compare("Glonass") == 0) { if (PRN_ < 1 or PRN_ > 24) { From 0caa7fff150ca02fc2b352fce5de8ae63ce7d449 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 30 Aug 2017 16:17:21 -0600 Subject: [PATCH 42/48] Fixes ineficiencies in the code, remove unused variables --- conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf | 14 ++--- .../libs/rtklib/rtklib_conversions.cc | 15 +++-- .../system_parameters/glonass_gnav_almanac.cc | 4 +- .../system_parameters/glonass_gnav_almanac.h | 4 +- .../glonass_gnav_ephemeris.cc | 14 ++--- .../glonass_gnav_ephemeris.h | 14 ++--- .../glonass_gnav_navigation_message.cc | 63 ++++++++++++------- .../glonass_gnav_navigation_message.h | 7 ++- src/core/system_parameters/rtcm.cc | 16 ++--- 9 files changed, 82 insertions(+), 69 deletions(-) diff --git a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf index 9b7c1cfa5..82f40644f 100644 --- a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf +++ b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf @@ -25,7 +25,7 @@ Resampler.sample_freq_out=6625000 Resampler.item_type=gr_complex ;######### CHANNELS GLOBAL CONFIG ############ -Channels_1G.count=2 +Channels_1G.count=5 Channels.in_acquisition=1 Channel0.signal=1G Channel1.signal=1G @@ -33,10 +33,10 @@ Channel2.signal=1G Channel3.signal=1G Channel4.signal=1G -Channel0.satellite=2 -Channel1.satellite=21 -Channel2.satellite=12 -Channel3.satellite=23 +;Channel0.satellite=2 +;Channel1.satellite=21 +;Channel2.satellite=12 +;Channel3.satellite=23 ; Possible list includes 2, 12, 21, 22 ;######### ACQUISITION GLOBAL CONFIG ############ @@ -48,7 +48,7 @@ Acquisition_1G.if=0 Acquisition_1G.doppler_max=10000 Acquisition_1G.doppler_step=250 Acquisition_1G.dump=false; -Acquisition_1G.dump_filename=../data/glo_acquisition.dat +Acquisition_1G.dump_filename=/archive/glo_acquisition.dat ;######### TRACKING GLOBAL CONFIG ############ Tracking_1G.implementation=GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking @@ -58,7 +58,7 @@ Tracking_1G.early_late_space_chips=0.5 Tracking_1G.pll_bw_hz=20.0; Tracking_1G.dll_bw_hz=4.0; Tracking_1G.dump=true; -Tracking_1G.dump_filename=../data/glo_tracking_ch_ +Tracking_1G.dump_filename=/archive/glo_tracking_ch_ ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1G.implementation=GPS_L1_CA_Telemetry_Decoder diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 45f13db82..c495fc210 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -72,7 +72,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) struct tm utcinfo; rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number; /* satellite number */ - rtklib_sat.iode = glonass_gnav_eph.d_t_b; /* IODE (0-6 bit of tb field) */ + rtklib_sat.iode = glonass_gnav_eph.d_iode; /* IODE (0-6 bit of tb field) */ rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */ rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ rtklib_sat.sva = glonass_gnav_eph.d_F_T; /* satellite accuracy*/ @@ -90,25 +90,24 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) rtklib_sat.gamn = glonass_gnav_eph.d_gamma_n; /* SV relative freq bias */ rtklib_sat.age = glonass_gnav_eph.d_Delta_tau_n; /* delay between L1 and L2 (s) */ - /* TODO: adjustment for week handover */ utcinfo.tm_mon = 0; - utcinfo.tm_mday = glonass_gnav_eph.d_D4Y; + utcinfo.tm_mday = glonass_gnav_eph.d_N_T; utcinfo.tm_year = glonass_gnav_eph.d_yr - 1900; - utcinfo.tm_hour = 0; + utcinfo.tm_hour = 6; // Diff between utc and (utc(su) + 3.00h) utcinfo.tm_min = 0; utcinfo.tm_sec = glonass_gnav_eph.d_t_b; t_utc.time = mktime(&utcinfo); - t_utc.sec = 0.0; + t_utc.sec = glonass_gnav_eph.d_tau_c; rtklib_sat.toe = utc2gpst(t_utc); /* epoch of epherides (gpst) */ utcinfo.tm_mon = 0; - utcinfo.tm_mday = glonass_gnav_eph.d_D4Y; + utcinfo.tm_mday = glonass_gnav_eph.d_N_T; utcinfo.tm_year = glonass_gnav_eph.d_yr - 1900; - utcinfo.tm_hour = 0; + utcinfo.tm_hour = 6; utcinfo.tm_min = 0; utcinfo.tm_sec = glonass_gnav_eph.d_t_k; t_utc.time = mktime(&utcinfo); - t_utc.sec = 0.0; + t_utc.sec = glonass_gnav_eph.d_tau_c; rtklib_sat.tof = utc2gpst(t_utc); /* message frame time (gpst) */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index cbef6df64..049d3a4c0 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -52,8 +52,8 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() d_M_n_A = 0.0; d_KP = 0.0; d_tau_n_A = 0.0; - d_C_n = 0.0; - d_l_n = 0.0; + d_C_n = false; + d_l_n = false; } void Glonass_Gnav_Almanac::satellite_position(double N_A, double N_i, double t_i) diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 2c7780bf5..3012708dd 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -59,8 +59,8 @@ public: double d_M_n_A; //!< Type of satellite n_A [dimensionless] double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless] double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] - double d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] - double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + bool d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] + bool d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] // Satellite Identification Information int i_satellite_freq_channel; //!< SV Frequency Channel Number diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index f5bb76eab..b7b8e2d3a 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -62,18 +62,16 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] d_E_n = 0.0; //!< Characterises "age" of a current information [days] d_P_1 = 0.0; //!< Flag of the immediate data updating [minutes] - d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] - d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] - d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - d_l3rd_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - d_l5th_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + d_P_2 = false; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + d_P_3 = false; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + d_P_4 = false; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + d_l3rd_n = false; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + d_l5th_n = false; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] // Satellite Identification Information i_satellite_freq_channel = 0; //!< SV Frequency Channel Number i_satellite_PRN = 0; //!< SV PRN Number, equivalent to slot number for compatibility with GPS i_satellite_slot_number = 0; //!< SV Slot Number - d_TOD = 0.0; //!< Time of Day of the ephemeris set based in start of frame [s] - d_D4Y = 0.0; //!< Day of Year after latest leap year (4 year interval) d_yr = 1972; //!< Current year, defaults to 1972 (UTC Epoch with leap seconds) d_satClkDrift = 0.0; //!< GLONASS clock error d_dtr = 0.0; //!< relativistic clock correction term @@ -86,7 +84,7 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() boost::posix_time::ptime Glonass_Gnav_Ephemeris::compute_GLONASS_time(const double offset_time) const { - boost::posix_time::time_duration t(0, 0, offset_time); + boost::posix_time::time_duration t(0, 0, offset_time + d_tau_c); boost::gregorian::date d1(d_yr, 1, 1); boost::gregorian::days d2(d_N_T); boost::posix_time::ptime glonass_time(d1+d2, t); diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index ef4a0ec12..281237b48 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -64,7 +64,7 @@ public: 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] - double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] + double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), double d_Xn; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. @@ -84,19 +84,17 @@ public: double d_Delta_tau_n; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless] double d_E_n; //!< Characterises "age" of a current information [days] double d_P_1; //!< Flag of the immediate data updating [minutes] - double d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] - double d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] - double d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] - double d_l3rd_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] - double 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] + bool d_P_2; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless] + bool d_P_3; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless] + bool d_P_4; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless] + bool d_l3rd_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + bool 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] // Inmediate deliverables of ephemeris information // Satellite Identification Information int i_satellite_freq_channel; //!< SV Frequency Channel Number unsigned int i_satellite_PRN; //!< SV PRN Number, equivalent to slot number for compatibility with GPS unsigned int i_satellite_slot_number; //!< SV Slot Number - double d_TOD; //!< Time of Day of the ephemeris set based in start of frame [s] - double d_D4Y; //!< Day of Year after latest leap year (4 year interval) double d_yr; //!< Current year double d_satClkDrift; //!< GLONASS clock error double d_dtr; //!< relativistic clock correction term diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index bd73917ce..424376129 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -74,12 +74,12 @@ void Glonass_Gnav_Navigation_Message::reset() //broadcast orbit 1 flag_TOW_set = false; - d_TOW = false; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - d_TOW_F1 = false; //!< Time of GPS Week from HOW word of Subframe 1 [s] - d_TOW_F2 = false; //!< Time of GPS Week from HOW word of Subframe 2 [s] - d_TOW_F3 = false; //!< Time of GPS Week from HOW word of Subframe 3 [s] - d_TOW_F4 = false; //!< Time of GPS Week from HOW word of Subframe 4 [s] - d_TOW_F5 = false; //!< Time of GPS Week from HOW word of Subframe 5 [s] + d_TOW = 0.0; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + d_TOW_F1 = 0.0; //!< Time of GPS Week from HOW word of Subframe 1 [s] + d_TOW_F2 = 0.0; //!< Time of GPS Week from HOW word of Subframe 2 [s] + d_TOW_F3 = 0.0; //!< Time of GPS Week from HOW word of Subframe 3 [s] + d_TOW_F4 = 0.0; //!< Time of GPS Week from HOW word of Subframe 4 [s] + d_TOW_F5 = 0.0; //!< Time of GPS Week from HOW word of Subframe 5 [s] // Clock terms d_satClkCorr = 0.0; @@ -310,6 +310,23 @@ unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int sate return frame_ID; } +double Glonass_Gnav_Navigation_Message::get_TOW() +{ + double TOW = 0.0; + double utcsu2utc = 3*3600; + double glot2utcsu = 3*3600; + int i = 0; + + TOW = gnav_ephemeris.d_t_k + glot2utcsu + utcsu2utc + gnav_utc_model.d_tau_c + gnav_utc_model.d_tau_gps; + + for (i = 0; leaps[i][0]>0; i++) + { + if (leaps[i][0] == gnav_ephemeris.d_yr) + { + TOW -= leaps[i][6]; + } + return TOW; +} int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) { @@ -337,6 +354,8 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * TWO_N11; flag_ephemeris_str_1 = true; + d_TOW = get_TOW(); + flag_TOW_set = true; break; @@ -345,7 +364,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) if (flag_ephemeris_str_1 == true) { gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); - gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); + gnav_ephemeris.d_P_2 = static_cast(read_navigation_bool(string_bits, P2)); gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B))*gnav_ephemeris.d_P_1*60; gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT))* TWO_N20; gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)) * TWO_N30; @@ -359,10 +378,10 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 3: // --- It is string 3 ---------------------------------------------- - gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); + gnav_ephemeris.d_P_3 = static_cast(read_navigation_bool(string_bits, P3)); gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)) * TWO_N40; gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); - gnav_ephemeris.d_l3rd_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); + gnav_ephemeris.d_l3rd_n = static_cast(read_navigation_bool(string_bits, EPH_L_N)); gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)) * TWO_N20; gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)) * TWO_N30; gnav_ephemeris.d_Zn = static_cast(read_navigation_signed(string_bits, Z_N)) * TWO_N11; @@ -376,7 +395,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_ephemeris.d_tau_n = static_cast(read_navigation_signed(string_bits, TAU_N)) * TWO_N30; gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)) * TWO_N30; gnav_ephemeris.d_E_n = static_cast(read_navigation_unsigned(string_bits, E_N)); - gnav_ephemeris.d_P_4 = static_cast(read_navigation_unsigned(string_bits, P4)); + gnav_ephemeris.d_P_4 = static_cast(read_navigation_bool(string_bits, P4)); gnav_ephemeris.d_F_T = static_cast(read_navigation_unsigned(string_bits, F_T)); gnav_ephemeris.d_N_T = static_cast(read_navigation_unsigned(string_bits, N_T)); gnav_ephemeris.d_n = static_cast(read_navigation_unsigned(string_bits, N)); @@ -385,7 +404,6 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) // Fill in ephemeris deliverables in the code gnav_ephemeris.i_satellite_slot_number = gnav_ephemeris.d_n; gnav_ephemeris.i_satellite_PRN = gnav_ephemeris.d_n; - gnav_ephemeris.d_D4Y = gnav_ephemeris.d_N_T; flag_ephemeris_str_4 = true; @@ -397,7 +415,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_utc_model.d_tau_c = static_cast(read_navigation_signed(string_bits, TAU_C)) * TWO_N31; gnav_utc_model.d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); gnav_utc_model.d_tau_gps = static_cast(read_navigation_signed(string_bits, TAU_GPS)) * TWO_N30; - gnav_ephemeris.d_l5th_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_ephemeris.d_l5th_n = static_cast(read_navigation_bool(string_bits, ALM_L_N)); flag_utc_model_str_5 = true; // Compute Year and DoY based on Algorithm A3.11 of GLONASS ICD @@ -422,6 +440,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) } // 2). Current year in common form is calculated by the following formula: gnav_ephemeris.d_yr = 1996 + 4.0*(gnav_utc_model.d_N_4 - 1.0) + (J - 1.0); + gnav_ephemeris.d_tau_c = gnav_utc_model.d_tau_c; } break; @@ -430,7 +449,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); frame_ID = get_frame_number(i_satellite_slot_number); - gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; @@ -451,7 +470,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_bool(string_bits, ALM_L_N)); // Set satellite information for redundancy purposes if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) @@ -475,7 +494,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); frame_ID = get_frame_number(i_satellite_slot_number); - gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; @@ -496,7 +515,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) -32.0; - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_bool(string_bits, ALM_L_N)); // Set satellite information for redundancy purposes if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) @@ -514,7 +533,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); frame_ID = get_frame_number(i_satellite_slot_number); - gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; @@ -535,7 +554,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_bool(string_bits, ALM_L_N)); // Set satellite information for redundancy purposes if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) @@ -553,7 +572,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); frame_ID = get_frame_number(i_satellite_slot_number); - gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; @@ -574,7 +593,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_bool(string_bits, ALM_L_N)); // Set satellite information for redundancy purposes if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) @@ -599,7 +618,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); frame_ID = get_frame_number(i_satellite_slot_number); - gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, n_A)); gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18; @@ -622,7 +641,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; - gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_bool(string_bits, ALM_L_N)); // Set satellite information for redundancy purposes if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 3cb73c819..0c71040f0 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -97,8 +97,6 @@ public: bool flag_utc_model_str_5; //!< Clock info send in string 5 of navigation data bool flag_utc_model_str_15; //!< Clock info send in string 15 of frame 5 of navigation data - //broadcast orbit 1 - //TODO Need to send the information regarding the frame number bool flag_TOW_set; double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s] @@ -156,6 +154,11 @@ public: */ int string_decoder(std::string frame_string); + /* + * \brief Gets the time of week in GPS Time + */ + double get_TOW(); + /*! * \brief Computes the Coordinated Universal Time (UTC) and returns it in [s] */ diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 7ee09e6bb..7dd60969a 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -3957,10 +3957,10 @@ int Rtcm::set_DF041(const Gnss_Synchro & gnss_synchro) return 0; } -//TODO Need to fix the lambda value since it needs to include frequency channel + int Rtcm::set_DF042(const Gnss_Synchro & gnss_synchro) { - const double lambda = GLONASS_C_m_s / GLONASS_L1_CA_FREQ_HZ; + const double lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 599584.92 ); double glonass_L1_pseudorange = std::round(( gnss_synchro.Pseudorange_m - ambiguity * 599584.92) / 0.02 ); double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 299792.458; @@ -4349,8 +4349,7 @@ int Rtcm::set_DF106(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) int Rtcm::set_DF107(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - // TODO Need to fix this here, bit position has a given order - unsigned int hrs = 0; + unsigned int hrs = 0; unsigned int min = 0; unsigned int sec = 0; unsigned int tk = 0; @@ -5271,8 +5270,7 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro) } if ((sig.compare("1C") == 0) && (sys.compare("R") == 0 )) { - // TODO Need to add slot number and freq number to gnss_syncro - lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ); + lambda = GLONASS_C_m_s / ((GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)))); } if ((sig.compare("2C") == 0) && (sys.compare("R") == 0 )) { @@ -5381,8 +5379,7 @@ int Rtcm::set_DF404(const Gnss_Synchro & gnss_synchro) } if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) { - //TODO Need to add slot number and freq number to gnss syncro - lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); } if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) { @@ -5470,8 +5467,7 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro) } if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) { - //TODO Need to add slot number and freq number to gnss syncro - lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); } if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) { From e5ffc2b46c4009e839a515b1937403286a06a63d Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 30 Aug 2017 22:44:03 -0600 Subject: [PATCH 43/48] Adding proper TOW computation for GLONASS processing --- src/core/system_parameters/GLONASS_L1_CA.h | 25 +++++++++++++++++++ .../glonass_gnav_navigation_message.cc | 17 +++++++++---- .../glonass_gnav_navigation_message.h | 4 ++- 3 files changed, 40 insertions(+), 6 deletions(-) diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 913f42fcd..56c208d31 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -88,6 +88,31 @@ const double GLONASS_L1_CA_CODE_PERIOD = 0.001; //!< GLONASS L1 C/ const double GLONASS_L1_CA_CHIP_PERIOD = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds] const double GLONASS_L1_CA_SYMBOL_RATE_BPS = 1000; +//FIXME Probably should use leap seconds definitions of rtklib +const double GLONASS_LEAP_SECONDS[21][7] = { /* leap seconds (y,m,d,h,m,s,utc-gpst) */ + {2019, 1, 1, 0, 0, 0, -20}, + {2018, 1, 1, 0, 0, 0, -19}, + {2017, 1, 1, 0, 0, 0, -18}, + {2015, 7, 1, 0, 0, 0, -17}, + {2012, 7, 1, 0, 0, 0, -16}, + {2009, 1, 1, 0, 0, 0, -15}, + {2006, 1, 1, 0, 0, 0, -14}, + {1999, 1, 1, 0, 0, 0, -13}, + {1997, 7, 1, 0, 0, 0, -12}, + {1996, 1, 1, 0, 0, 0, -11}, + {1994, 7, 1, 0, 0, 0, -10}, + {1993, 7, 1, 0, 0, 0, -9}, + {1992, 7, 1, 0, 0, 0, -8}, + {1991, 1, 1, 0, 0, 0, -7}, + {1990, 1, 1, 0, 0, 0, -6}, + {1988, 1, 1, 0, 0, 0, -5}, + {1985, 7, 1, 0, 0, 0, -4}, + {1983, 7, 1, 0, 0, 0, -3}, + {1982, 7, 1, 0, 0, 0, -2}, + {1981, 7, 1, 0, 0, 0, -1}, + {} +}; + // GLONASS SV's orbital slots PRN = (orbital_slot - 1) const std::map GLONASS_PRN = {{ 0, 8,}, //For test diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 424376129..5149d5b46 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -319,11 +319,12 @@ double Glonass_Gnav_Navigation_Message::get_TOW() TOW = gnav_ephemeris.d_t_k + glot2utcsu + utcsu2utc + gnav_utc_model.d_tau_c + gnav_utc_model.d_tau_gps; - for (i = 0; leaps[i][0]>0; i++) + for (i = 0; GLONASS_LEAP_SECONDS[i][0]>0; i++) { - if (leaps[i][0] == gnav_ephemeris.d_yr) + if (GLONASS_LEAP_SECONDS[i][0] == gnav_ephemeris.d_yr) { - TOW -= leaps[i][6]; + TOW -= GLONASS_LEAP_SECONDS[i][6]; + } } return TOW; } @@ -354,8 +355,6 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)) * TWO_N11; flag_ephemeris_str_1 = true; - d_TOW = get_TOW(); - flag_TOW_set = true; break; @@ -441,6 +440,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) // 2). Current year in common form is calculated by the following formula: gnav_ephemeris.d_yr = 1996 + 4.0*(gnav_utc_model.d_N_4 - 1.0) + (J - 1.0); gnav_ephemeris.d_tau_c = gnav_utc_model.d_tau_c; + + // 3). Set TOW once the year has been defined, it helps with leap second determination + if(flag_ephemeris_str_1 == true) + { + d_TOW = get_TOW(); + flag_TOW_set = true; + } + } break; diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 0c71040f0..651ccbebd 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -156,9 +156,11 @@ public: /* * \brief Gets the time of week in GPS Time + * \details This converts from GLONASS Time to GPS Time of Week based on the + * start of frame */ double get_TOW(); - + /*! * \brief Computes the Coordinated Universal Time (UTC) and returns it in [s] */ From 6b8973efe29fe66ff5a65d0ec75f330b17542d43 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Thu, 31 Aug 2017 16:51:50 -0600 Subject: [PATCH 44/48] Fixing bugs in telemetry decoder code --- conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf | 10 ++--- .../glonass_l1_ca_telemetry_decoder_cc.cc | 24 +++++++----- src/core/system_parameters/GLONASS_L1_CA.h | 2 +- .../glonass_gnav_navigation_message.cc | 39 ++++++++++++------- .../glonass_gnav_navigation_message.h | 3 +- src/core/system_parameters/gnss_satellite.cc | 2 +- src/core/system_parameters/rtcm.cc | 8 ++-- .../matlab/gps_l1_ca_dll_pll_plot_sample.m | 8 ++-- 8 files changed, 55 insertions(+), 41 deletions(-) diff --git a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf index 82f40644f..51f5b0096 100644 --- a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf +++ b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf @@ -12,7 +12,7 @@ SignalSource.sampling_frequency=6625000 ;SignalSource.samples=66250000 SignalSource.samples=0 SignalSource.dump=false; -SignalSource.dump_filename=./signal_glonass.bin +SignalSource.dump_filename=/archive/signal_glonass.bin ;######### SIGNAL_CONDITIONER CONFIG ############ SignalConditioner.implementation=Signal_Conditioner @@ -33,8 +33,8 @@ Channel2.signal=1G Channel3.signal=1G Channel4.signal=1G -;Channel0.satellite=2 -;Channel1.satellite=21 +Channel0.satellite=11 +Channel1.satellite=15 ;Channel2.satellite=12 ;Channel3.satellite=23 ; Possible list includes 2, 12, 21, 22 @@ -43,7 +43,7 @@ Channel4.signal=1G Acquisition_1G.implementation=GLONASS_L1_CA_PCPS_Acquisition Acquisition_1G.item_type=gr_complex Acquisition_1G.threshold=0.0 -Acquisition_1G.pfa=0.01 +Acquisition_1G.pfa=0.0001 Acquisition_1G.if=0 Acquisition_1G.doppler_max=10000 Acquisition_1G.doppler_step=250 @@ -61,7 +61,7 @@ Tracking_1G.dump=true; Tracking_1G.dump_filename=/archive/glo_tracking_ch_ ;######### TELEMETRY DECODER GPS CONFIG ############ -TelemetryDecoder_1G.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1G.implementation=GLONASS_L1_CA_Telemetry_Decoder ;######### OBSERVABLES CONFIG ############ Observables.implementation=Hybrid_Observables diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index 21c549b88..894bac175 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -145,15 +145,19 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int chip_acc += frame_symbols[i]; chip_acc_counter += 1; - if(chip_acc_counter == (GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT - 1)) + if(chip_acc_counter == (GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT)) { if (chip_acc > 0) { bi_binary_code.push_back('1'); + chip_acc_counter = 0; + chip_acc = 0; } else { bi_binary_code.push_back('0'); + chip_acc_counter = 0; + chip_acc = 0; } } } @@ -171,18 +175,16 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int } // 2. Call the GLONASS GNAV string decoder - d_nav.string_decoder(relative_code.c_str()); + d_nav.string_decoder(relative_code); // 3. Check operation executed correctly if(d_nav.flag_CRC_test == true) { - LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite; - std::cout << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite << std::endl; + LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite ";// << d_satellite; } else { - std::cout << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite << std::endl; - LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite; + LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " ;//<< d_satellite; } // 4. Push the new navigation data to the queues @@ -225,7 +227,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib consume_each(1); d_flag_preamble = false; - unsigned int required_symbols=GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS; + unsigned int required_symbols=GLONASS_GNAV_STRING_SYMBOLS; if (d_symbol_history.size()>required_symbols) { @@ -264,7 +266,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib if (abs(preamble_diff - GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) == 0) { //try to decode frame - LOG(INFO) << "Starting page decoder for GLONASS L1 C/A SAT " << this->d_satellite; + LOG(INFO) << "Starting string decoder for GLONASS L1 C/A SAT " << this->d_satellite; d_preamble_index = d_sample_counter; //record the preamble sample stamp d_stat = 2; } @@ -274,17 +276,19 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib { d_stat = 0; // start again } + DLOG(INFO) << "Failed string decoder for GLONASS L1 C/A SAT " << this->d_satellite; } } } else if (d_stat == 2) { - if (d_sample_counter == d_preamble_index + GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS) + // FIXME: The preamble index marks the first symbol of the string count. Here I just wait for another full string to be received before processing + if (d_sample_counter == d_preamble_index + GLONASS_GNAV_STRING_SYMBOLS) { // NEW GLONASS string received // 0. fetch the symbols into an array int string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; - double string_symbols[string_length]; + double string_symbols[string_length] = {0}; //******* SYMBOL TO BIT ******* for (int i = 0; i < string_length; i++) diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 56c208d31..f5c4d450a 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -151,7 +151,7 @@ const int GLONASS_L1_CA_HISTORY_DEEP = 100; const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3; const int GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30; const int GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300; -const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; +const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 2000; const int GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10; const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10; diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 5149d5b46..636605064 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -81,6 +81,10 @@ void Glonass_Gnav_Navigation_Message::reset() d_TOW_F4 = 0.0; //!< Time of GPS Week from HOW word of Subframe 4 [s] d_TOW_F5 = 0.0; //!< Time of GPS Week from HOW word of Subframe 5 [s] + flag_CRC_test = false; + d_frame_ID = 0; + d_string_ID = 0; + // Clock terms d_satClkCorr = 0.0; d_dtr = 0.0; @@ -310,6 +314,7 @@ unsigned int Glonass_Gnav_Navigation_Message::get_frame_number(unsigned int sate return frame_ID; } + double Glonass_Gnav_Navigation_Message::get_TOW() { double TOW = 0.0; @@ -329,20 +334,21 @@ double Glonass_Gnav_Navigation_Message::get_TOW() return TOW; } + int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) { - int string_ID = 0; int J = 0; - frame_ID = 0; + d_string_ID = 0; + d_frame_ID = 0; // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE std::bitset string_bits = std::bitset((frame_string)); - string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); + d_string_ID = static_cast(read_navigation_unsigned(string_bits, STRING_ID)); - CRC_test(string_bits); + flag_CRC_test = CRC_test(string_bits); // Decode all 15 string messages - switch (string_ID) + switch (d_string_ID) { case 1: //--- It is string 1 ----------------------------------------------- @@ -454,7 +460,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 6: // --- It is string 6 ---------------------------------------------- i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); - frame_ID = get_frame_number(i_satellite_slot_number); + d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); @@ -499,7 +505,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 8: // --- It is string 8 ---------------------------------------------- i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); - frame_ID = get_frame_number(i_satellite_slot_number); + d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); @@ -538,7 +544,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 10: // --- It is string 10 --------------------------------------------- i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); - frame_ID = get_frame_number(i_satellite_slot_number); + d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); @@ -577,7 +583,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 12: // --- It is string 12 --------------------------------------------- i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); - frame_ID = get_frame_number(i_satellite_slot_number); + d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); @@ -615,7 +621,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) break; case 14: // --- It is string 14 --------------------------------------------- - if( frame_ID == 5) + if( d_frame_ID == 5) { gnav_utc_model.d_B1 = static_cast(read_navigation_unsigned(string_bits, B1)); gnav_utc_model.d_B2 = static_cast(read_navigation_unsigned(string_bits, B2)); @@ -623,7 +629,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) else { i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); - frame_ID = get_frame_number(i_satellite_slot_number); + d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); @@ -641,7 +647,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 15: // --- It is string 9 ---------------------------------------------- - if (frame_ID != 5 and flag_almanac_str_14 == true ) + if (d_frame_ID != 5 and flag_almanac_str_14 == true ) { gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15; gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; @@ -662,10 +668,13 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) } break; default: - break; - } // switch subframeID ... + LOG(INFO) << "GLONASS GNAV: Invalid String ID of received. Received " << d_string_ID << ", but acceptable range is from 1-15"; - return frame_ID; + + break; + } // switch string ID ... + + return d_string_ID; } diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 651ccbebd..6a8cf194e 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -61,7 +61,8 @@ private: public: bool flag_CRC_test; - unsigned int frame_ID; + unsigned int d_frame_ID; + unsigned int d_string_ID; Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index a64ba740d..7fb9c38ec 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -356,7 +356,7 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int } - if (system_.compare("GLONASS") == 0) + if (system_.compare("Glonass") == 0) { switch ( PRN_ ) { diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 7dd60969a..20c5141b2 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -3960,7 +3960,7 @@ int Rtcm::set_DF041(const Gnss_Synchro & gnss_synchro) int Rtcm::set_DF042(const Gnss_Synchro & gnss_synchro) { - const double lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); + const double lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 599584.92 ); double glonass_L1_pseudorange = std::round(( gnss_synchro.Pseudorange_m - ambiguity * 599584.92) / 0.02 ); double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 299792.458; @@ -5270,7 +5270,7 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro) } if ((sig.compare("1C") == 0) && (sys.compare("R") == 0 )) { - lambda = GLONASS_C_m_s / ((GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)))); + lambda = GLONASS_C_m_s / ((GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)))); } if ((sig.compare("2C") == 0) && (sys.compare("R") == 0 )) { @@ -5379,7 +5379,7 @@ int Rtcm::set_DF404(const Gnss_Synchro & gnss_synchro) } if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) { - lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); } if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) { @@ -5467,7 +5467,7 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro) } if ((sig_.compare("1C") == 0) && (sys_.compare("R") == 0 )) { - lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); + lambda = GLONASS_C_m_s / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))); } if ((sig_.compare("2C") == 0) && (sys_.compare("R") == 0 )) { diff --git a/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m b/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m index c1c884eaa..fed82d412 100644 --- a/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m +++ b/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m @@ -35,14 +35,14 @@ if ~exist('gps_l1_ca_dll_pll_read_tracking_dump.m','file') end -samplingFreq = 2600000; %[Hz] -channels = 2; +samplingFreq = 6625000; %[Hz] +channels = 5; first_channel = 0; -path = '/home/javier/git/gnss-sdr/build/'; %% CHANGE THIS PATH +path = '/archive/'; %% CHANGE THIS PATH for N=1:1:channels - tracking_log_path = [path 'tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename + tracking_log_path = [path 'glo_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename GNSS_tracking(N)= gps_l1_ca_dll_pll_read_tracking_dump(tracking_log_path); end From a3d2c563c9e7378f470af44246882c6a2269b07e Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Thu, 31 Aug 2017 20:51:40 -0600 Subject: [PATCH 45/48] Fixing bugs and adding plotting tool for GLONASS measurements Adds tracking results plot for GLONASS processing following code for GPS. Also fix a couple of bugs in the code and address bugs from acq and trk modules --- .../glonass_l1_ca_telemetry_decoder_cc.cc | 6 +- .../glonass_gnav_navigation_message.cc | 10 +- .../glonass_gnav_navigation_message.h | 2 +- .../glonass_l1_ca_dll_pll_plot_sample.m | 73 +++++++ ...glonass_l1_ca_dll_pll_read_tracking_dump.m | 193 ++++++++++++++++++ 5 files changed, 275 insertions(+), 9 deletions(-) create mode 100644 src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m create mode 100644 src/utils/matlab/libs/glonass_l1_ca_dll_pll_read_tracking_dump.m diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index 894bac175..5cc51b758 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -180,11 +180,11 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int // 3. Check operation executed correctly if(d_nav.flag_CRC_test == true) { - LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite ";// << d_satellite; + LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite "<< d_satellite; } else { - LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " ;//<< d_satellite; + LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues @@ -399,7 +399,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib void glonass_l1_ca_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite) { d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; + DLOG(INFO) << "Setting decoder Finite State Machine to satellite "<< d_satellite; DLOG(INFO) << "Navigation Satellite set to " << d_satellite; } diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 636605064..f92dcf57a 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -459,7 +459,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) case 6: // --- It is string 6 ---------------------------------------------- - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); @@ -504,7 +504,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) break; case 8: // --- It is string 8 ---------------------------------------------- - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); @@ -543,7 +543,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) break; case 10: // --- It is string 10 --------------------------------------------- - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); @@ -582,7 +582,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) break; case 12: // --- It is string 12 --------------------------------------------- - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); @@ -628,7 +628,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) } else { - i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); + i_satellite_slot_number = static_cast(read_navigation_unsigned(string_bits, n_A)); d_frame_ID = get_frame_number(i_satellite_slot_number); gnav_almanac[i_satellite_slot_number - 1].d_C_n = static_cast(read_navigation_bool(string_bits, C_N)); diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 6a8cf194e..04836fc62 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -155,7 +155,7 @@ public: */ int string_decoder(std::string frame_string); - /* + /*! * \brief Gets the time of week in GPS Time * \details This converts from GLONASS Time to GPS Time of Week based on the * start of frame diff --git a/src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m b/src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m new file mode 100644 index 000000000..18965a31d --- /dev/null +++ b/src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m @@ -0,0 +1,73 @@ +% /*! +% * \file glonass_l1_ca_dll_pll_plot_sample.m +% * \brief Read GNSS-SDR Tracking dump binary file using the provided +% function and plot some internal variables +% * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * 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 . +% * +% * ------------------------------------------------------------------------- +% */ +close all; +clear all; + +if ~exist('glonass_l1_ca_dll_pll_read_tracking_dump.m','file') + addpath('./libs') +end + + +samplingFreq = 6625000; %[Hz] +channels = 5; +first_channel = 0; + +path = '/archive/'; %% CHANGE THIS PATH + +for N=1:1:channels + tracking_log_path = [path 'glo_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename + GNSS_tracking(N)= gps_l1_ca_dll_pll_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; + trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; + + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E)); + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E); + plotTracking(N,trackResults,settings) +end diff --git a/src/utils/matlab/libs/glonass_l1_ca_dll_pll_read_tracking_dump.m b/src/utils/matlab/libs/glonass_l1_ca_dll_pll_read_tracking_dump.m new file mode 100644 index 000000000..8b1e54243 --- /dev/null +++ b/src/utils/matlab/libs/glonass_l1_ca_dll_pll_read_tracking_dump.m @@ -0,0 +1,193 @@ +% /*! +% * \file glonass_l1_ca_dll_pll_read_tracking_dump.m +% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB. +% * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * 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 . +% * +% * ------------------------------------------------------------------------- +% */ +function [GNSS_tracking] = glonass_l1_ca_dll_pll_read_tracking_dump (filename, count) + + %% usage: glonass_l1_ca_dll_pll_read_tracking_dump_64bits (filename, [count]) + %% + %% open GNSS-SDR tracking binary log file .dat and return the contents + %% + + m = nargchk (1,2,nargin); + num_float_vars=5; + num_unsigned_long_int_vars=1; + num_double_vars=11; + num_unsigned_int_vars=1; + double_size_bytes=8; + unsigned_long_int_size_bytes=8; + float_size_bytes=4; + long_int_size_bytes=4; + + skip_bytes_each_read=float_size_bytes*num_float_vars+unsigned_long_int_size_bytes*num_unsigned_long_int_vars+double_size_bytes*num_double_vars+long_int_size_bytes*num_unsigned_int_vars; + bytes_shift=0; + if (m) + usage (m); + end + + if (nargin < 2) + %count = Inf; + file_stats = dir(filename); + %round num bytes to read to integer number of samples (to protect the script from binary + %dump end file transitory) + count = (file_stats.bytes - mod(file_stats.bytes,skip_bytes_each_read))/skip_bytes_each_read; + end + %loops_counter = fread (f, count, 'uint32',4*12); + f = fopen (filename, 'rb'); + if (f < 0) + else + v1 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v2 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v3 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v4 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v5 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved unsigned_long_int + v6 = fread (f, count, 'uint64',skip_bytes_each_read-unsigned_long_int_size_bytes); + bytes_shift=bytes_shift+unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v7 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v8 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v9 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v10 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v11 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v12 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v13 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v14 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v15 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v16 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v17 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v18 = fread (f, count, 'uint32',skip_bytes_each_read-double_size_bytes); + fclose (f); + + %%%%%%%% output vars %%%%%%%% + +% // EPR +% d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); +% d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); +% d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); +% // PROMPT I and Q (to analyze navigation symbols) +% d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); +% d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); +% // PRN start sample stamp +% //tmp_float=(float)d_sample_counter; +% d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); +% // accumulated carrier phase +% d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); +% +% // carrier and code frequency +% d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); +% +% //PLL commands +% d_dump_file.write(reinterpret_cast(&carr_phase_error_secs_Ti), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); +% +% //DLL commands +% d_dump_file.write(reinterpret_cast(&code_error_chips_Ti), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); +% +% // CN0 and carrier lock test +% d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); +% +% // AUX vars (for debug purposes) +% tmp_double = d_rem_code_phase_samples; +% d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); +% tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); +% d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); +% // PRN +% unsigned int prn_ = d_acquisition_gnss_synchro->PRN; +% d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + E=v1; + P=v2; + L=v3; + prompt_I=v4; + prompt_Q=v5; + PRN_start_sample=v6; + acc_carrier_phase_rad=v7; + carrier_doppler_hz=v8; + code_freq_hz=v9; + carr_error=v10; + carr_nco=v11; + code_error=v12; + code_nco=v13; + CN0_SNV_dB_Hz=v14; + carrier_lock_test=v15; + var1=v16; + var2=v17; + PRN=v18; + + GNSS_tracking.E=E; + GNSS_tracking.P=P; + GNSS_tracking.L=L; + GNSS_tracking.prompt_I=prompt_I; + GNSS_tracking.prompt_Q=prompt_Q; + GNSS_tracking.PRN_start_sample=PRN_start_sample; + GNSS_tracking.acc_carrier_phase_rad=acc_carrier_phase_rad; + GNSS_tracking.carrier_doppler_hz=carrier_doppler_hz; + GNSS_tracking.code_freq_hz=code_freq_hz; + GNSS_tracking.carr_error=carr_error; + GNSS_tracking.carr_nco=carr_nco; + GNSS_tracking.code_error=code_error + GNSS_tracking.code_nco=code_nco; + GNSS_tracking.CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; + GNSS_tracking.carrier_lock_test=carrier_lock_test; + GNSS_tracking.d_rem_code_phase_samples=var1; + GNSS_tracking.var2=var2; + GNSS_tracking.PRN=PRN; + end From 778fa4f5f818121fcc3208235788be108150cd57 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Thu, 7 Sep 2017 21:59:27 -0600 Subject: [PATCH 46/48] bugfix: Fixes bug for carrier frequency in pcps_acq for GLONASS The code fixes the computation of the carrier frequency for pcps acquisition of GLONASS satellites by making it relative to the IF. Also new plotting features are added to visualize results --- .gitignore | 2 + conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf | 2 +- conf/gnss-sdr_GLONASS_L1_ibyte.conf | 103 ---------- .../gnuradio_blocks/pcps_acquisition_cc.cc | 4 +- src/core/receiver/gnss_flowgraph.cc | 26 +-- ...ple.m => glonass_ca_dll_pll_plot_sample.m} | 8 +- .../glonass_ca_dll_pll_read_tracking_dump.m | 191 ++++++++++++++++++ 7 files changed, 213 insertions(+), 123 deletions(-) delete mode 100644 conf/gnss-sdr_GLONASS_L1_ibyte.conf rename src/utils/matlab/{glonass_l1_ca_dll_pll_plot_sample.m => glonass_ca_dll_pll_plot_sample.m} (89%) create mode 100644 src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m diff --git a/.gitignore b/.gitignore index cbccacdac..a128f1558 100644 --- a/.gitignore +++ b/.gitignore @@ -9,5 +9,7 @@ thirdparty/ .settings .project .cproject +.idea +cmake-build-debug/ /install .DS_Store diff --git a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf index 51f5b0096..a7b6f6551 100644 --- a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf +++ b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf @@ -61,7 +61,7 @@ Tracking_1G.dump=true; Tracking_1G.dump_filename=/archive/glo_tracking_ch_ ;######### TELEMETRY DECODER GPS CONFIG ############ -TelemetryDecoder_1G.implementation=GLONASS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1G.implementation=GPS_L1_CA_Telemetry_Decoder ;######### OBSERVABLES CONFIG ############ Observables.implementation=Hybrid_Observables diff --git a/conf/gnss-sdr_GLONASS_L1_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_ibyte.conf deleted file mode 100644 index 0b16fa3d8..000000000 --- a/conf/gnss-sdr_GLONASS_L1_ibyte.conf +++ /dev/null @@ -1,103 +0,0 @@ -; You can define your own receiver and invoke it by doing -; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf -; - -[GNSS-SDR] - -;######### GLOBAL OPTIONS ################## -;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. -GNSS-SDR.internal_fs_hz=6625000 - -;######### CONTROL_THREAD CONFIG ############ -ControlThread.wait_for_flowgraph=false - -;######### SIGNAL_SOURCE CONFIG ############ -SignalSource.implementation=File_Signal_Source -SignalSource.filename=/home/dmiralles/Documents/GSOC/GSOC2017/gnss-sdr/data/dmirallesNT1065_L2_20160831_fs6625e6_60e3_schar_1H.bin -SignalSource.item_type=ibyte -SignalSource.sampling_frequency=6625000 -SignalSource.freq=1602000000 -SignalSource.samples=0 -SignalSource.repeat=false -SignalSource.sample_type=iq -SignalSource.seconds_to_skip=0 -SignalSource.dump=false -SignalSource.dump_filename=../data/signal_source.dat -SignalSource.enable_throttle_control=false - - -;######### SIGNAL_CONDITIONER CONFIG ############ -SignalConditioner.implementation=Signal_Conditioner - -;DataTypeAdapter.implementation=Ishort_To_Complex -DataTypeAdapter.implementation=Ibyte_To_Complex -InputFilter.implementation=Pass_Through -;InputFilter.input_item_type=gr_complex -;InputFilter.output_item_type=gr_complex -InputFilter.item_type=gr_complex -;Resampler.implementation=Pass_Through -;Resampler.item_type=gr_complex -Resampler.implementation=Pass_Through -;Resampler.sample_freq_in=4000000 -;Resampler.sample_freq_out=2000000 -;Resampler.item_type=gr_complex -Resampler.item_type=gr_complex - -;######### CHANNELS GLOBAL CONFIG ############ -Channels_1R.count=8 ;Assuming here that identifier `1r=R` defines GLONASS SP signals -Channels.in_acquisition=1 -Channel.signal=1R -;Channel.item_type=cshort - - -;######### ACQUISITION GLOBAL CONFIG ############ -Acquisition_1R.dump=false -Acquisition_1R.dump_filename=./acq_dump.dat -Acquisition_1R.item_type=cshort -Acquisition_1R.if=0 -Acquisition_1R.sampled_ms=1 -Acquisition_1R.implementation=GLONASS_L1_CA_PCPS_Acquisition -Acquisition_1R.threshold=0.008 -;Acquisition_1C.pfa=0.000001 -Acquisition_1R.doppler_max=10000 -Acquisition_1R.doppler_step=250 -Acquisition_1R.tong_init_val=2 -Acquisition_1R.tong_max_val=10 -Acquisition_1R.tong_max_dwells=20 - -;######### TRACKING GLOBAL CONFIG ############ -Tracking_1R.implementation=GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking -Tracking_1R.item_type=cshort -Tracking_1R.if=0 -Tracking_1R.dump=false -Tracking_1R.dump_filename=../data/epl_tracking_ch_ -Tracking_1R.pll_bw_hz=40.0; -Tracking_1R.dll_bw_hz=4.0; -Tracking_1R.order=3; - -;######### TELEMETRY DECODER GPS CONFIG ############ -TelemetryDecoder_1R.implementation=GLONASS_L1_CA_Telemetry_Decoder -TelemetryDecoder_1R.dump=false -TelemetryDecoder_1R.decimation_factor=1; - -;######### OBSERVABLES CONFIG ############ -Observables.implementation=Hybrid_Observables -Observables.dump=false -Observables.dump_filename=./observables.dat - - -;######### PVT CONFIG ############ -PVT.implementation=RTKLIB_PVT -PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic -PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX -PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad -PVT.output_rate_ms=100 -PVT.display_rate_ms=500 -PVT.dump_filename=./PVT -PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; -PVT.flag_nmea_tty_port=false; -PVT.nmea_dump_devname=/dev/pts/4 -PVT.flag_rtcm_server=false -PVT.flag_rtcm_tty_port=false -PVT.rtcm_dump_devname=/dev/pts/1 -PVT.dump=false diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index cd314a342..6911cc958 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -194,8 +194,8 @@ bool pcps_acquisition_cc::is_fdma() // Dealing with FDMA system if( strcmp(d_gnss_synchro->Signal,"1G") == 0 ) { - d_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); - LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN) << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; + d_freq += d_old_freq * GLONASS_PRN.at(d_gnss_synchro->PRN); + LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_old_freq * GLONASS_PRN.at(d_gnss_synchro->PRN) << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; return true; } else diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 4529a1fba..6fae5b7b4 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -735,19 +735,19 @@ void GNSSFlowgraph::set_signals_list() } } - if (configuration_->property("Channels_1G.count", 0) > 0 ) - { - /* - * Loop to create the list of GLONASS L1 C/A signals - */ - for (available_gnss_prn_iter = available_glonass_prn.begin(); - available_gnss_prn_iter != available_glonass_prn.end(); - available_gnss_prn_iter++) - { - available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Glonass"), - *available_gnss_prn_iter), std::string("1G"))); - } - } + if (configuration_->property("Channels_1G.count", 0) > 0 ) + { + /* + * Loop to create the list of GLONASS L1 C/A signals + */ + for (available_gnss_prn_iter = available_glonass_prn.begin(); + available_gnss_prn_iter != available_glonass_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Glonass"), + *available_gnss_prn_iter), std::string("1G"))); + } + } /* * Ordering the list of signals from configuration file */ diff --git a/src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m b/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m similarity index 89% rename from src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m rename to src/utils/matlab/glonass_ca_dll_pll_plot_sample.m index 18965a31d..2e2c9b070 100644 --- a/src/utils/matlab/glonass_l1_ca_dll_pll_plot_sample.m +++ b/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m @@ -1,5 +1,5 @@ % /*! -% * \file glonass_l1_ca_dll_pll_plot_sample.m +% * \file glonass_ca_dll_pll_plot_sample.m % * \brief Read GNSS-SDR Tracking dump binary file using the provided % function and plot some internal variables % * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com @@ -30,7 +30,7 @@ close all; clear all; -if ~exist('glonass_l1_ca_dll_pll_read_tracking_dump.m','file') +if ~exist('glonass_ca_dll_pll_read_tracking_dump.m','file') addpath('./libs') end @@ -43,7 +43,7 @@ path = '/archive/'; %% CHANGE THIS PATH for N=1:1:channels tracking_log_path = [path 'glo_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename - GNSS_tracking(N)= gps_l1_ca_dll_pll_read_tracking_dump(tracking_log_path); + GNSS_tracking(N)= glonass_ca_dll_pll_read_tracking_dump(tracking_log_path); end % GNSS-SDR format conversion to MATLAB GPS receiver @@ -69,5 +69,5 @@ for N=1:1:channels % Use original MATLAB tracking plot function settings.numberOfChannels = channels; settings.msToProcess = length(GNSS_tracking(N).E); - plotTracking(N,trackResults,settings) + plotTracking(N,trackResults,settings); end diff --git a/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m b/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m new file mode 100644 index 000000000..098ee11f7 --- /dev/null +++ b/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m @@ -0,0 +1,191 @@ +% /*! +% * \file glonass_ca_dll_pll_read_tracking_dump.m +% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB. +% * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * 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 . +% * +% * ------------------------------------------------------------------------- +% */ +function [GNSS_tracking] = glonass_ca_dll_pll_read_tracking_dump (filename, count) + + %% usage: gps_l1_ca_dll_pll_read_tracking_dump_64bits (filename, [count]) + %% + %% open GNSS-SDR tracking binary log file .dat and return the contents + %% + + narginchk (1,2); + num_float_vars=5; + num_unsigned_long_int_vars=1; + num_double_vars=11; + num_unsigned_int_vars=1; + double_size_bytes=8; + unsigned_long_int_size_bytes=8; + float_size_bytes=4; + long_int_size_bytes=4; + + skip_bytes_each_read=float_size_bytes*num_float_vars+unsigned_long_int_size_bytes*num_unsigned_long_int_vars+double_size_bytes*num_double_vars+long_int_size_bytes*num_unsigned_int_vars; + bytes_shift=0; + + if (nargin < 2) + %count = Inf; + file_stats = dir(filename); + %round num bytes to read to integer number of samples (to protect the script from binary + %dump end file transitory) + count = (file_stats.bytes - mod(file_stats.bytes,skip_bytes_each_read))/skip_bytes_each_read; + end + %loops_counter = fread (f, count, 'uint32',4*12); + f = fopen (filename, 'rb'); + if (f < 0) + else + v1 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v2 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v3 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v4 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v5 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes); + bytes_shift=bytes_shift+float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved unsigned_long_int + v6 = fread (f, count, 'uint64',skip_bytes_each_read-unsigned_long_int_size_bytes); + bytes_shift=bytes_shift+unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v7 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v8 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v9 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v10 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v11 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v12 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v13 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v14 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v15 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v16 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v17 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved double + v18 = fread (f, count, 'uint32',skip_bytes_each_read-double_size_bytes); + fclose (f); + + %%%%%%%% output vars %%%%%%%% + +% // EPR +% d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); +% d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); +% d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); +% // PROMPT I and Q (to analyze navigation symbols) +% d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); +% d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); +% // PRN start sample stamp +% //tmp_float=(float)d_sample_counter; +% d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); +% // accumulated carrier phase +% d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); +% +% // carrier and code frequency +% d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); +% +% //PLL commands +% d_dump_file.write(reinterpret_cast(&carr_phase_error_secs_Ti), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); +% +% //DLL commands +% d_dump_file.write(reinterpret_cast(&code_error_chips_Ti), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); +% +% // CN0 and carrier lock test +% d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); +% d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); +% +% // AUX vars (for debug purposes) +% tmp_double = d_rem_code_phase_samples; +% d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); +% tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); +% d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); +% // PRN +% unsigned int prn_ = d_acquisition_gnss_synchro->PRN; +% d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + E=v1; + P=v2; + L=v3; + prompt_I=v4; + prompt_Q=v5; + PRN_start_sample=v6; + acc_carrier_phase_rad=v7; + carrier_doppler_hz=v8; + code_freq_hz=v9; + carr_error=v10; + carr_nco=v11; + code_error=v12; + code_nco=v13; + CN0_SNV_dB_Hz=v14; + carrier_lock_test=v15; + var1=v16; + var2=v17; + PRN=v18; + + GNSS_tracking.E=E; + GNSS_tracking.P=P; + GNSS_tracking.L=L; + GNSS_tracking.prompt_I=prompt_I; + GNSS_tracking.prompt_Q=prompt_Q; + GNSS_tracking.PRN_start_sample=PRN_start_sample; + GNSS_tracking.acc_carrier_phase_rad=acc_carrier_phase_rad; + GNSS_tracking.carrier_doppler_hz=carrier_doppler_hz; + GNSS_tracking.code_freq_hz=code_freq_hz; + GNSS_tracking.carr_error=carr_error; + GNSS_tracking.carr_nco=carr_nco; + GNSS_tracking.code_error=code_error; + GNSS_tracking.code_nco=code_nco; + GNSS_tracking.CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; + GNSS_tracking.carrier_lock_test=carrier_lock_test; + GNSS_tracking.d_rem_code_phase_samples=var1; + GNSS_tracking.var2=var2; + GNSS_tracking.PRN=PRN; + end + From 2946d36f846ff9505f480bfab540a0a58d4ae9a3 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sat, 9 Sep 2017 13:44:35 -0600 Subject: [PATCH 47/48] bugfix: Solves shared freq channel ambiguity and carr freq in plots Adds code to solve the shared frequency channels for GLONASS satellites during acq and trk, it is now decode responsability to fix the identifier when decoding of ephemeris happens. It also adds more details in tracking plotting and fixes silly bug that created false alarm. --- conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf | 16 +++--- conf/gnss-sdr_GPS_L1_gr_complex.conf | 3 +- .../gnuradio_blocks/pcps_acquisition_cc.cc | 4 +- .../glonass_l1_ca_telemetry_decoder_cc.cc | 2 +- ...glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 5 +- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.h | 2 +- src/core/receiver/gnss_flowgraph.cc | 4 +- src/core/system_parameters/GLONASS_L1_CA.h | 1 + .../galileo_e1_dll_pll_veml_plot_sample.m | 1 + .../matlab/glonass_ca_dll_pll_plot_sample.m | 3 +- .../matlab/gps_l1_ca_dll_pll_plot_sample.m | 1 + .../glonass_ca_dll_pll_read_tracking_dump.m | 4 +- src/utils/matlab/libs/plotTracking.m | 49 ++++++++++++++++--- 13 files changed, 69 insertions(+), 26 deletions(-) diff --git a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf index a7b6f6551..4f3d513dc 100644 --- a/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf +++ b/conf/gnss-sdr_GLONASS_L1_CA_ibyte.conf @@ -26,7 +26,7 @@ Resampler.item_type=gr_complex ;######### CHANNELS GLOBAL CONFIG ############ Channels_1G.count=5 -Channels.in_acquisition=1 +Channels.in_acquisition=5 Channel0.signal=1G Channel1.signal=1G Channel2.signal=1G @@ -34,9 +34,10 @@ Channel3.signal=1G Channel4.signal=1G Channel0.satellite=11 -Channel1.satellite=15 -;Channel2.satellite=12 -;Channel3.satellite=23 +Channel1.satellite=2 +Channel2.satellite=18 +Channel3.satellite=12 +Channel4.satellite=21 ; Possible list includes 2, 12, 21, 22 ;######### ACQUISITION GLOBAL CONFIG ############ @@ -49,14 +50,15 @@ Acquisition_1G.doppler_max=10000 Acquisition_1G.doppler_step=250 Acquisition_1G.dump=false; Acquisition_1G.dump_filename=/archive/glo_acquisition.dat +;Acquisition_1G.coherent_integration_time_ms=10 ;######### TRACKING GLOBAL CONFIG ############ Tracking_1G.implementation=GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking Tracking_1G.item_type=gr_complex -Tracking_1G.if=0 +Tracking_1G.if=1 Tracking_1G.early_late_space_chips=0.5 -Tracking_1G.pll_bw_hz=20.0; -Tracking_1G.dll_bw_hz=4.0; +Tracking_1G.pll_bw_hz=25.0; +Tracking_1G.dll_bw_hz=3.0; Tracking_1G.dump=true; Tracking_1G.dump_filename=/archive/glo_tracking_ch_ diff --git a/conf/gnss-sdr_GPS_L1_gr_complex.conf b/conf/gnss-sdr_GPS_L1_gr_complex.conf index ca5af458c..c24eeae40 100644 --- a/conf/gnss-sdr_GPS_L1_gr_complex.conf +++ b/conf/gnss-sdr_GPS_L1_gr_complex.conf @@ -34,7 +34,7 @@ DataTypeAdapter.dump=false DataTypeAdapter.dump_filename=../data/DataTypeAdapter.dat ;######### CHANNELS GLOBAL CONFIG ############ -Channels_1C.count=8 +Channels_1C.count=5 Channels.in_acquisition=1 Channel.signal=1C @@ -51,6 +51,7 @@ Acquisition_1C.threshold=0.05 Acquisition_1C.doppler_max=10000 Acquisition_1C.doppler_step=250 + ;######### TRACKING GLOBAL CONFIG ############ Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking Tracking_1C.item_type=gr_complex diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index 6911cc958..b61c6d4a1 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -194,8 +194,8 @@ bool pcps_acquisition_cc::is_fdma() // Dealing with FDMA system if( strcmp(d_gnss_synchro->Signal,"1G") == 0 ) { - d_freq += d_old_freq * GLONASS_PRN.at(d_gnss_synchro->PRN); - LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_old_freq * GLONASS_PRN.at(d_gnss_synchro->PRN) << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; + d_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); + LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; return true; } else diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index 5cc51b758..f80748f80 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -288,7 +288,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib // NEW GLONASS string received // 0. fetch the symbols into an array int string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; - double string_symbols[string_length] = {0}; + double string_symbols[GLONASS_GNAV_DATA_SYMBOLS] = {0}; //******* SYMBOL TO BIT ******* for (int i = 0; i < string_length; i++) 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 42f80725e..367bcb371 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 @@ -196,7 +196,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * static_cast(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN))); double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler double T_chip_mod_seconds; @@ -576,7 +576,8 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __a d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + double if_freq_carrier = d_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * static_cast(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN))); + d_dump_file.write(reinterpret_cast(&if_freq_carrier), sizeof(double)); d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); //PLL commands 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 280d590b1..262707535 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 @@ -85,7 +85,7 @@ private: long d_if_freq; long d_fs_in; - long d_glonass_freq_ch; + double d_glonass_freq_ch; double d_early_late_spc_chips; int d_n_correlator_taps; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 6fae5b7b4..c83bf1e92 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -597,8 +597,8 @@ void GNSSFlowgraph::set_signals_list() 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36}; - std::set available_glonass_prn = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, - 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24}; + // Removing satellites sharing same frequency number(1 and 5, 2 and 6, 3 and 7, 4 and 6, 11 and 15, 12 and 16, 14 and 18, 17 and 21 + std::set available_glonass_prn = { 1, 2, 3, 4, 9, 10, 11, 12, 18, 19, 20, 21 }; std::string sv_list = configuration_->property("Galileo.prns", std::string("") ); diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index f5c4d450a..ca71ccd08 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -159,6 +159,7 @@ const int GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RA const int GLONASS_GNAV_STRING_SYMBOLS = 2000; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] const int GLONASS_GNAV_STRING_BITS = 85; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits] const int GLONASS_GNAV_HAMMING_CODE_BITS = 8; //!< Number of bits in hamming code sequence of GNAV message +const int GLONASS_GNAV_DATA_SYMBOLS = 1700; // STRING DATA WITHOUT PREAMBLE const std::vector GLONASS_GNAV_CRC_I_INDEX {9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84}; const std::vector GLONASS_GNAV_CRC_J_INDEX {9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84}; diff --git a/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m b/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m index 7e543b180..19bc3de13 100644 --- a/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m +++ b/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m @@ -68,6 +68,7 @@ for N=1:1:channels trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L)); trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; % Use original MATLAB tracking plot function settings.numberOfChannels = channels; diff --git a/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m b/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m index 2e2c9b070..7cb6753b4 100644 --- a/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m +++ b/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m @@ -51,7 +51,7 @@ end for N=1:1:channels trackResults(N).status = 'T'; %fake track trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; - trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_freq_hz.'; trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; @@ -64,6 +64,7 @@ for N=1:1:channels trackResults(N).I_L = GNSS_tracking(N).L.'; trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E)); % Use original MATLAB tracking plot function diff --git a/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m b/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m index fed82d412..22ef63f79 100644 --- a/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m +++ b/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m @@ -65,6 +65,7 @@ for N=1:1:channels trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E)); + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; % Use original MATLAB tracking plot function settings.numberOfChannels = channels; diff --git a/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m b/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m index 098ee11f7..7141d1970 100644 --- a/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m +++ b/src/utils/matlab/libs/glonass_ca_dll_pll_read_tracking_dump.m @@ -157,7 +157,7 @@ function [GNSS_tracking] = glonass_ca_dll_pll_read_tracking_dump (filename, coun prompt_Q=v5; PRN_start_sample=v6; acc_carrier_phase_rad=v7; - carrier_doppler_hz=v8; + carrier_freq_hz=v8; code_freq_hz=v9; carr_error=v10; carr_nco=v11; @@ -176,7 +176,7 @@ function [GNSS_tracking] = glonass_ca_dll_pll_read_tracking_dump (filename, coun GNSS_tracking.prompt_Q=prompt_Q; GNSS_tracking.PRN_start_sample=PRN_start_sample; GNSS_tracking.acc_carrier_phase_rad=acc_carrier_phase_rad; - GNSS_tracking.carrier_doppler_hz=carrier_doppler_hz; + GNSS_tracking.carrier_freq_hz=carrier_freq_hz; GNSS_tracking.code_freq_hz=code_freq_hz; GNSS_tracking.carr_error=carr_error; GNSS_tracking.carr_nco=carr_nco; diff --git a/src/utils/matlab/libs/plotTracking.m b/src/utils/matlab/libs/plotTracking.m index d2e31f537..ec0c418d3 100644 --- a/src/utils/matlab/libs/plotTracking.m +++ b/src/utils/matlab/libs/plotTracking.m @@ -54,15 +54,20 @@ for channelNr = channelList %% Draw axes ============================================================== % Row 1 - handles(1, 1) = subplot(3, 3, 1); - handles(1, 2) = subplot(3, 3, [2 3]); + handles(1, 1) = subplot(4, 3, 1); + handles(1, 2) = subplot(4, 3, [2 3]); % Row 2 - handles(2, 1) = subplot(3, 3, 4); - handles(2, 2) = subplot(3, 3, [5 6]); + handles(2, 1) = subplot(4, 3, 4); + handles(2, 2) = subplot(4, 3, [5 6]); % Row 3 - handles(3, 1) = subplot(3, 3, 7); - handles(3, 2) = subplot(3, 3, 8); - handles(3, 3) = subplot(3, 3, 9); + handles(3, 1) = subplot(4, 3, 7); + handles(3, 2) = subplot(4, 3, 8); + handles(3, 3) = subplot(4, 3, 9); + % Row 4 + handles(4, 1) = subplot(4, 3, 10); + handles(4, 2) = subplot(4, 3, 11); + handles(4, 3) = subplot(4, 3, 12); + %% Plot all figures ======================================================= @@ -149,5 +154,35 @@ for channelNr = channelList xlabel(handles(3, 3), 'Time (s)'); ylabel(handles(3, 3), 'Amplitude'); title (handles(3, 3), 'Filtered DLL discriminator'); + + %----- CNo for signal---------------------------------- + plot (handles(4, 1), timeAxisInSeconds, ... + trackResults(channelNr).CNo(1:settings.msToProcess), 'b'); + + grid (handles(4, 1)); + axis (handles(4, 1), 'tight'); + xlabel(handles(4, 1), 'Time (s)'); + ylabel(handles(4, 1), 'CNo (dB-Hz)'); + title (handles(4, 1), 'Carrier to Noise Ratio'); + + %----- Carrier Frequency -------------------------------- + plot (handles(4, 2), timeAxisInSeconds, ... + trackResults(channelNr).carrFreq(1:settings.msToProcess), 'Color',[0.42 0.25 0.39]); + + grid (handles(4, 2)); + axis (handles(4, 2), 'tight'); + xlabel(handles(4, 2), 'Time (s)'); + ylabel(handles(4, 2), 'Freq (hz)'); + title (handles(4, 2), 'Carrier Freq'); + + %----- Code Frequency---------------------------------- + plot (handles(4, 3), timeAxisInSeconds, ... + trackResults(channelNr).codeFreq(1:settings.msToProcess), 'Color',[0.2 0.3 0.49]); + + grid (handles(4, 3)); + axis (handles(4, 3), 'tight'); + xlabel(handles(4, 3), 'Time (s)'); + ylabel(handles(4, 3), 'Freq (Hz)'); + title (handles(4, 3), 'Code Freq'); end % for channelNr = channelList From e60c93da7204d0c5e06b547211d080aa527253a6 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 10 Sep 2017 11:04:53 +0200 Subject: [PATCH 48/48] Remove extraneous parentheses warnings --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 4 ++-- src/core/system_parameters/glonass_gnav_navigation_message.cc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 083aee259..f5bf5e503 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -920,11 +920,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { rp->log_rinex_nav(rp->navGloFile, d_ls_pvt->glonass_gnav_ephemeris_map); } - if((type_of_rx == 26)) // GPS L1 C/A + GLONASS L1 C/A + if(type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->glonass_gnav_ephemeris_map); } - if((type_of_rx == 27)) // Galileo E1B + GLONASS L1 C/A + if(type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->galileo_ephemeris_map, d_ls_pvt->glonass_gnav_ephemeris_map); } diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index f92dcf57a..7555324ef 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -709,7 +709,7 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a { if ((flag_ephemeris_str_1 == true) and (flag_ephemeris_str_2 == true) and (flag_ephemeris_str_3 == true) and (flag_ephemeris_str_4 == true)) { - if ((gnav_ephemeris.d_P_4 == 1) ) + if (gnav_ephemeris.d_P_4 == 1) { flag_ephemeris_str_1 = false;// clear the flag flag_ephemeris_str_2 = false;// clear the flag