diff --git a/src/algorithms/PVT/libs/rtcm.cc b/src/algorithms/PVT/libs/rtcm.cc index 4da88d870..94cf673af 100644 --- a/src/algorithms/PVT/libs/rtcm.cc +++ b/src/algorithms/PVT/libs/rtcm.cc @@ -3397,6 +3397,33 @@ std::vector Rtcm::print_IGM03(const Galileo_HAS_data& has_data) } +std::vector Rtcm::print_IGM05(const Galileo_HAS_data& has_data) +{ + std::vector msgs; + const uint8_t nsys = has_data.Nsys; + bool ssr_multiple_msg_indicator = true; + for (uint8_t sys = 0; sys < nsys; sys++) + { + if (sys == nsys - 1) + { + ssr_multiple_msg_indicator = false; // last message of a sequence + } + const std::string header = Rtcm::get_IGM05_header(has_data, sys, ssr_multiple_msg_indicator); + const std::string sat_data = Rtcm::get_IGM05_content_sat(has_data, sys); + if (!sat_data.empty()) + { + std::string message = build_message(header + sat_data); + if (server_is_running) + { + rtcm_message_queue->push(message); + } + msgs.push_back(message); + } + } + return msgs; +} + + std::string Rtcm::get_IGM01_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator) { std::string header; @@ -3630,6 +3657,212 @@ std::string Rtcm::get_IGM03_content_sat(const Galileo_HAS_data& has_data, uint8_ } +std::string Rtcm::get_IGM05_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator) +{ + std::string header; + + uint32_t tow = 0; // TODO + uint16_t ssr_provider_id = 0; // ? + uint8_t igm_version = 0; // ? + uint8_t ssr_solution_id = 0; // ? + auto iod_ssr = has_data.header.iod_id % 15; // ?? HAS IOD is 0-31 + + uint8_t subtype_msg_number = 0; + if (has_data.gnss_id_mask[nsys] == 0) // GPS + { + subtype_msg_number = 25; + } + else if (has_data.gnss_id_mask[nsys] == 2) // Galileo + { + subtype_msg_number = 65; + } + + uint8_t validity_index = has_data.validity_interval_index_orbit_corrections; + uint16_t validity_seconds = has_data.get_validity_interval_s(validity_index); + uint8_t ssr_update_interval_ = ssr_update_interval(validity_seconds); + uint8_t Nsat = has_data.get_num_satellites()[nsys]; + + Rtcm::set_DF002(4076); // Always “4076” for IGS Proprietary Messages + Rtcm::set_IDF001(igm_version); + Rtcm::set_IDF002(subtype_msg_number); + Rtcm::set_IDF003(tow); + Rtcm::set_IDF004(ssr_update_interval_); + Rtcm::set_IDF005(ssr_multiple_msg_indicator); + Rtcm::set_IDF007(iod_ssr); + Rtcm::set_IDF008(ssr_provider_id); + Rtcm::set_IDF009(ssr_solution_id); + Rtcm::set_IDF010(Nsat); + + header += DF002.to_string() + IDF001.to_string() + IDF002.to_string() + + IDF003.to_string() + IDF004.to_string() + IDF005.to_string() + + IDF007.to_string() + IDF008.to_string() + IDF009.to_string() + + IDF010.to_string(); + return header; +} + + +std::string Rtcm::get_IGM05_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index) +{ + std::string content; + + const uint8_t num_sats_in_this_system = has_data.get_num_satellites()[nsys_index]; + std::vector prn = has_data.get_PRNs_in_mask(nsys_index); + std::vector> code_bias_m = has_data.get_code_bias_m(); + + for (uint8_t sat = 0; sat < num_sats_in_this_system; sat++) + { + uint8_t num_bias_processed = has_data.get_signals_in_mask(nsys_index).size(); + + uint8_t valid_num_bias_processed = 0; + std::vector gnss_signal_tracking_mode_id_v; + std::vector valid_bias_v; + + for (uint8_t code = 0; code < num_bias_processed; code++) + { + std::string code_string = has_data.get_signals_in_mask(nsys_index)[code]; + if (has_data.gnss_id_mask[nsys_index] == 0) // GPS + { + if (code_string == "L1 C/A") + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L1 L1C(D)") + { + gnss_signal_tracking_mode_id_v.push_back(3); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L1 L1C(P)") + { + gnss_signal_tracking_mode_id_v.push_back(4); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L2 L2C(M)") + { + gnss_signal_tracking_mode_id_v.push_back(7); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L2 L2C(L)") + { + gnss_signal_tracking_mode_id_v.push_back(8); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L5 I") + { + gnss_signal_tracking_mode_id_v.push_back(14); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L5 Q") + { + gnss_signal_tracking_mode_id_v.push_back(15); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(false); + } + } + else if (has_data.gnss_id_mask[nsys_index] == 2) // Galileo + { + if (code_string == "E1-B I/NAV OS") + { + gnss_signal_tracking_mode_id_v.push_back(1); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E1-C") + { + gnss_signal_tracking_mode_id_v.push_back(2); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5a-I F/NAV OS") + { + gnss_signal_tracking_mode_id_v.push_back(5); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5a-Q") + { + gnss_signal_tracking_mode_id_v.push_back(6); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5b-I I/NAV OS") + { + gnss_signal_tracking_mode_id_v.push_back(8); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5b-Q") + { + gnss_signal_tracking_mode_id_v.push_back(9); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E6-B C/NAV HAS") + { + gnss_signal_tracking_mode_id_v.push_back(15); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E6-C") + { + gnss_signal_tracking_mode_id_v.push_back(16); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(false); + } + } + else + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(false); + } + } + + if (valid_num_bias_processed > 0) + { + Rtcm::set_IDF011(static_cast(prn[sat])); + Rtcm::set_IDF023(valid_num_bias_processed); + + content += IDF011.to_string() + IDF023.to_string(); + + uint8_t num_sats_in_previous_systems = 0; + for (uint8_t nsys = 0; nsys < nsys_index; nsys++) + { + num_sats_in_previous_systems += has_data.get_num_satellites()[nsys]; + } + uint8_t sat_index = sat + num_sats_in_previous_systems; + + for (uint8_t code = 0; code < num_bias_processed; code++) + { + if (valid_bias_v[code] == true) + { + Rtcm::set_IDF024(gnss_signal_tracking_mode_id_v[code]); + Rtcm::set_IDF025(code_bias_m[sat_index][code]); + content += DF024.to_string() + IDF025.to_string(); + } + } + } + } + + return content; +} + + // ***************************************************************************************************** // Some utilities // ***************************************************************************************************** diff --git a/src/algorithms/PVT/libs/rtcm.h b/src/algorithms/PVT/libs/rtcm.h index f5e0cfa0c..2dcee5f50 100644 --- a/src/algorithms/PVT/libs/rtcm.h +++ b/src/algorithms/PVT/libs/rtcm.h @@ -352,6 +352,11 @@ public: */ std::vector print_IGM03(const Galileo_HAS_data& has_data); + /*! + * \brief Prints messages of type IGM05 (SSR Bias Correction) + */ + std::vector print_IGM05(const Galileo_HAS_data& has_data); + uint32_t lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); //!< Returns the time period in which GPS L1 signals have been continually tracked. uint32_t lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); //!< Returns the time period in which GPS L2 signals have been continually tracked. uint32_t lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); //!< Returns the time period in which Galileo signals have been continually tracked. @@ -498,6 +503,8 @@ private: std::string get_IGM02_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); std::string get_IGM03_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator); std::string get_IGM03_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); + std::string get_IGM05_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator); + std::string get_IGM05_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); // // Utilities