From f720ddf20876ad7d1c7b504719e8d5c7a48127bc Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 13 Aug 2017 09:58:35 -0700 Subject: [PATCH] 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; }