diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index ec546ebeb..f49f912c4 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -26,6 +26,7 @@ #include "galileo_utc_model.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_utc_model.h" +#include "gnss_satellite.h" #include "gnss_sdr_filesystem.h" #include "gnss_synchro.h" #include "gps_cnav_ephemeris.h" @@ -152,6 +153,12 @@ bool is_qzss_prn(uint32_t prn) } +bool is_gps_prn(uint32_t prn) +{ + return prn >= MINPRNGPS && prn <= MAXPRNGPS; +} + + boost::posix_time::ptime gps_time_to_ptime(int32_t gps_week, double tow_s) { const gtime_t time = gpst2time(gps_week, tow_s); @@ -2566,6 +2573,28 @@ void Rinex_Printer::log_rinex_nav_gps_nav(const std::map { const auto& eph = gps_ephemeris_iter.second; + const bool key_matches_prn = gps_ephemeris_iter.first >= 0 && static_cast(gps_ephemeris_iter.first) == eph.PRN; + if (!key_matches_prn || (!is_gps_prn(eph.PRN) && !is_qzss_prn(eph.PRN))) + { + LOG(WARNING) << "Skipping malformed GPS/QZSS LNAV ephemeris in RINEX output: map key=" + << gps_ephemeris_iter.first << ", PRN=" << eph.PRN; + continue; + } + + const auto satellite_block_iter = eph.satelliteBlock.find(eph.PRN); + std::string satellite_block; + if (satellite_block_iter != eph.satelliteBlock.cend()) + { + satellite_block = satellite_block_iter->second; + } + else + { + const std::string system = is_qzss_prn(eph.PRN) ? "QZSS" : "GPS"; + satellite_block = Gnss_Satellite().what_block(system, eph.PRN); + LOG(WARNING) << "Missing satellite block metadata for " << system << " PRN " << eph.PRN + << " in RINEX output; reconstructed block as " << satellite_block; + } + // -------- SV / EPOCH / SV CLK const boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_GPS_time(eph, eph.toc); @@ -2651,7 +2680,7 @@ void Rinex_Printer::log_rinex_nav_gps_nav(const std::map int curve_fit_interval = 4; - if (eph.satelliteBlock.at(eph.PRN) == "IIA") + if (satellite_block == "IIA") { // Block II/IIA (Table 20-XI IS-GPS-200M) if ((eph.IODC > 239) && (eph.IODC < 248)) @@ -2680,10 +2709,10 @@ void Rinex_Printer::log_rinex_nav_gps_nav(const std::map } } - if ((eph.satelliteBlock.at(eph.PRN) == "IIR") || - (eph.satelliteBlock.at(eph.PRN) == "IIR-M") || - (eph.satelliteBlock.at(eph.PRN) == "IIF") || - (eph.satelliteBlock.at(eph.PRN) == "III")) + if ((satellite_block == "IIR") || + (satellite_block == "IIR-M") || + (satellite_block == "IIF") || + (satellite_block == "III")) { // Block IIR/IIR-M/IIF/III/IIIF (Table 20-XII IS-GPS-200M) if ((eph.IODC > 239) && (eph.IODC < 248)) diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index a1f779e72..25fa353a4 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -20,6 +20,7 @@ #include "gnss_sdr_supl_client.h" #include "GPS_L1_CA.h" #include "MATH_CONSTANTS.h" +#include "gnss_satellite.h" #include #include #include @@ -36,6 +37,14 @@ #include #endif +namespace +{ +constexpr uint32_t GPS_LNAV_MIN_PRN = 1U; +constexpr uint32_t GPS_LNAV_MAX_PRN = 32U; +constexpr uint32_t QZSS_LNAV_MIN_PRN = 193U; +constexpr uint32_t QZSS_LNAV_MAX_PRN = 206U; +} // namespace + Gnss_Sdr_Supl_Client::Gnss_Sdr_Supl_Client() : server_port(0), request(0), mcc(0), mns(0), lac(0), ci(0) { @@ -44,6 +53,34 @@ Gnss_Sdr_Supl_Client::Gnss_Sdr_Supl_Client() } +bool Gnss_Sdr_Supl_Client::is_gps_lnav_prn(uint32_t prn) +{ + return prn >= GPS_LNAV_MIN_PRN && prn <= GPS_LNAV_MAX_PRN; +} + + +bool Gnss_Sdr_Supl_Client::is_qzss_lnav_prn(uint32_t prn) +{ + return prn >= QZSS_LNAV_MIN_PRN && prn <= QZSS_LNAV_MAX_PRN; +} + + +void Gnss_Sdr_Supl_Client::restore_gps_lnav_ephemeris_metadata(Gps_Ephemeris& ephemeris) +{ + const bool is_qzss = is_qzss_lnav_prn(ephemeris.PRN); + const std::string system = is_qzss ? "QZSS" : "GPS"; + const uint32_t first_prn = is_qzss ? QZSS_LNAV_MIN_PRN : GPS_LNAV_MIN_PRN; + const uint32_t last_prn = is_qzss ? QZSS_LNAV_MAX_PRN : GPS_LNAV_MAX_PRN; + + ephemeris.set_system(is_qzss ? 'J' : 'G'); + Gnss_Satellite satellite; + for (uint32_t prn = first_prn; prn <= last_prn; ++prn) + { + ephemeris.satelliteBlock[prn] = satellite.what_block(system, prn); + } +} + + void Gnss_Sdr_Supl_Client::print_assistance() { if (assist.set & SUPL_RRLP_ASSIST_REFTIME) @@ -373,20 +410,54 @@ void Gnss_Sdr_Supl_Client::read_supl_data() bool Gnss_Sdr_Supl_Client::load_ephemeris_xml(const std::string& file_name) { + gps_ephemeris_map.clear(); + std::map loaded_ephemeris_map; std::ifstream ifs; try { ifs.open(file_name.c_str(), std::ifstream::binary | std::ifstream::in); + if (!ifs.is_open()) + { + LOG(WARNING) << "Failed to open ephemeris XML file: " << file_name; + return false; + } boost::archive::xml_iarchive xml(ifs); - gps_ephemeris_map.clear(); - xml >> boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", this->gps_ephemeris_map); - LOG(INFO) << "Loaded Ephemeris map data with " << this->gps_ephemeris_map.size() << " satellites"; + xml >> boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", loaded_ephemeris_map); } - catch (std::exception& e) + catch (const std::exception& e) { - LOG(WARNING) << e.what() << "File: " << file_name; + LOG(WARNING) << e.what() << ". File: " << file_name; return false; } + + size_t rejected_records = 0; + for (auto& item : loaded_ephemeris_map) + { + const int map_key = item.first; + Gps_Ephemeris& ephemeris = item.second; + const bool key_matches_prn = map_key >= 0 && static_cast(map_key) == ephemeris.PRN; + const bool valid_prn = is_gps_lnav_prn(ephemeris.PRN) || is_qzss_lnav_prn(ephemeris.PRN); + + if (!key_matches_prn || !valid_prn) + { + LOG(WARNING) << "Rejected malformed GPS/QZSS LNAV ephemeris from " << file_name + << ": map key=" << map_key << ", PRN=" << ephemeris.PRN; + ++rejected_records; + continue; + } + + restore_gps_lnav_ephemeris_metadata(ephemeris); + gps_ephemeris_map.emplace(map_key, std::move(ephemeris)); + } + + if (gps_ephemeris_map.empty()) + { + LOG(WARNING) << "No valid GPS/QZSS LNAV ephemeris records found in " << file_name; + return false; + } + + LOG(INFO) << "Loaded Ephemeris map data with " << gps_ephemeris_map.size() << " satellites" + << (rejected_records == 0 ? "" : ", rejected " + std::to_string(rejected_records) + " malformed record(s)"); return true; } diff --git a/src/core/libs/gnss_sdr_supl_client.h b/src/core/libs/gnss_sdr_supl_client.h index b37b68da3..0dae0f080 100644 --- a/src/core/libs/gnss_sdr_supl_client.h +++ b/src/core/libs/gnss_sdr_supl_client.h @@ -39,6 +39,7 @@ extern "C" #include "gps_ephemeris.h" #include "gps_iono.h" #include "gps_utc_model.h" +#include #include #include #include @@ -255,6 +256,9 @@ public: void print_assistance(); private: + static bool is_gps_lnav_prn(uint32_t prn); + static bool is_qzss_lnav_prn(uint32_t prn); + static void restore_gps_lnav_ephemeris_metadata(Gps_Ephemeris& ephemeris); bool read_gal_almanac_from_gsa(const std::string& file_name); // assistance protocol structure supl_ctx_t ctx{}; diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 34b0196e5..48f11b27e 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -557,7 +557,7 @@ bool ControlThread::read_assistance_from_XML() std::cout << "Trying to read GNSS ephemeris from XML file(s)...\n"; - if (configuration_->property("Channels_1C.count", 0) > 0) + if ((configuration_->property("Channels_1C.count", 0) > 0) || (configuration_->property("Channels_J1.count", 0) > 0)) { if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true) { @@ -566,7 +566,8 @@ bool ControlThread::read_assistance_from_XML() gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.cend(); gps_eph_iter++) { - std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.PRN) << '\n'; + const std::string system = gps_eph_iter->second.get_system() == 'J' ? "QZSS" : "GPS"; + std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite(system, gps_eph_iter->second.PRN) << '\n'; const std::shared_ptr tmp_obj = std::make_shared(gps_eph_iter->second); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); } diff --git a/src/core/system_parameters/gps_ephemeris.h b/src/core/system_parameters/gps_ephemeris.h index 2e878c459..0c4d56861 100644 --- a/src/core/system_parameters/gps_ephemeris.h +++ b/src/core/system_parameters/gps_ephemeris.h @@ -45,6 +45,16 @@ public: */ Gps_Ephemeris(); + void set_system(char system) + { + this->System = system; + } + + char get_system() const + { + return this->System; + } + int32_t code_on_L2{}; //!< If 1, P code ON in L2; if 2, C/A code ON in L2; bool L2_P_data_flag{}; //!< When true, indicates that the NAV data stream was commanded OFF on the P-code of the L2 channel int32_t SV_accuracy{}; //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200M) diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index 274b4ad83..4fc5e5d48 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -622,6 +622,7 @@ Gps_Ephemeris Gps_Navigation_Message::get_ephemeris() const ephemeris.antispoofing_flag = b_antispoofing_flag; if (d_system == LnavSystem::QZSS) { + ephemeris.set_system('J'); auto gnss_sat = Gnss_Satellite(); for (uint32_t i = 193; i < 207; i++) {