.. _program_listing_file_interfaces_sp3_loader.cc: Program Listing for File sp3_loader.cc ====================================== |exhale_lsh| :ref:`Return to documentation for file ` (``interfaces/sp3_loader.cc``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp #include "lupnt/interfaces/sp3_loader.h" #include #include #include #include #include #include #include #include #include #include #include "lupnt/conversions/time_conversions.h" #include "lupnt/core/error.h" #include "lupnt/core/file.h" #include "lupnt/numerics/interpolation.h" namespace lupnt { namespace { constexpr double kC = 299792458.0; constexpr double kSecsWeek = 7.0 * SECS_DAY; struct Sp3ProductInfo { int gps_week = 0; int day_of_week = 0; int year = 0; int doy = 0; std::string filename; std::string zipped_filename; std::string url; }; std::string ShellQuote(const std::string& s) { std::string out = "'"; for (char c : s) { if (c == '\'') { out += "'\\''"; } else { out += c; } } out += "'"; return out; } bool IsTruthyEnv(const char* value) { if (value == nullptr) return false; std::string s(value); std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) { return static_cast(std::tolower(c)); }); return !(s.empty() || s == "0" || s == "false" || s == "off" || s == "no"); } bool IsSp3DownloadDisabled() { return IsTruthyEnv(std::getenv("LUPNT_SKIP_SP3_DOWNLOAD")); } bool RunShellCommand(const std::string& cmd) { return std::system(cmd.c_str()) == 0; } bool LooksLikeHtml(const std::filesystem::path& filepath) { std::ifstream file(filepath, std::ios::binary); if (!file.is_open()) return false; std::string head(256, '\0'); file.read(head.data(), static_cast(head.size())); head.resize(static_cast(file.gcount())); auto first = std::find_if_not(head.begin(), head.end(), [](unsigned char c) { return std::isspace(c) != 0; }); std::string trimmed(first, head.end()); std::transform(trimmed.begin(), trimmed.end(), trimmed.begin(), [](unsigned char c) { return static_cast(std::tolower(c)); }); return trimmed.rfind(" 0; if (!ok) { std::filesystem::remove(tmp_path, ec); return false; } std::filesystem::rename(tmp_path, dest_path, ec); if (ec) { ec.clear(); std::filesystem::copy_file(tmp_path, dest_path, std::filesystem::copy_options::overwrite_existing, ec); std::filesystem::remove(tmp_path, ec); } return !ec; } void GunzipToFile(const std::filesystem::path& gzip_path, const std::filesystem::path& dest_path) { std::filesystem::path tmp_path = dest_path; tmp_path += ".part"; std::error_code ec; std::filesystem::remove(tmp_path, ec); std::string cmd = fmt::format("gzip -dc {} > {}", ShellQuote(gzip_path.string()), ShellQuote(tmp_path.string())); LUPNT_CHECK(RunShellCommand(cmd) && std::filesystem::exists(tmp_path) && std::filesystem::file_size(tmp_path) > 0, "Failed to decompress SP3 gzip file: " + gzip_path.string(), "Sp3Loader"); std::filesystem::rename(tmp_path, dest_path, ec); if (ec) { ec.clear(); std::filesystem::copy_file(tmp_path, dest_path, std::filesystem::copy_options::overwrite_existing, ec); std::filesystem::remove(tmp_path, ec); } LUPNT_CHECK(!ec, "Failed to write decompressed SP3 file: " + dest_path.string(), "Sp3Loader"); } int DayOfYear(int year, int month, int day) { static constexpr std::array kMonthStartsCommon = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}; bool leap = (year % 4 == 0 && year % 100 != 0) || (year % 400 == 0); return kMonthStartsCommon.at(static_cast(month - 1)) + day + (leap && month > 2 ? 1 : 0); } Sp3ProductInfo BuildSp3ProductInfo(Real epoch, Time time_scale) { const Real t_gps = ConvertTime(epoch, time_scale, Time::GPS); const Real gps_epoch = GregorianToTime(1980, 1, 6, 0, 0, 0.0); const double delta = t_gps.val() - gps_epoch.val(); LUPNT_CHECK(delta >= 0.0, "SP3 download helper does not support epochs before GPS epoch", "Sp3Loader"); Sp3ProductInfo info; info.gps_week = static_cast(std::floor(delta / kSecsWeek)); info.day_of_week = static_cast(std::floor((delta - info.gps_week * kSecsWeek) / SECS_DAY)); LUPNT_CHECK(info.gps_week >= 1962, "C++ SP3 download helper supports modern CDDIS COD/MGEX `.SP3.gz` products " "(GPS week >= 1962) only", "Sp3Loader"); auto [year, month, day, hour, minute, second] = MjdToGregorian(TimeToMjd(t_gps)); (void)hour; (void)minute; (void)second; info.year = year; info.doy = DayOfYear(year, month, day); info.filename = fmt::format("COD0MGXFIN_{:04d}{:03d}0000_01D_05M_ORB.SP3", info.year, info.doy); info.zipped_filename = info.filename + ".gz"; info.url = fmt::format("https://cddis.nasa.gov/archive/gnss/products/{:04d}/{}", info.gps_week, info.zipped_filename); return info; } } // namespace // Constructors *************************************************************** Sp3Loader::Sp3Loader(const std::filesystem::path& filepath) { LoadFile(filepath); } Sp3Loader::Sp3Loader(const std::vector& filepaths) { for (const auto& filepath : filepaths) LoadFile(filepath); } std::string Sp3Loader::FilenameForEpoch(Real epoch, Time time_scale) { return BuildSp3ProductInfo(epoch, time_scale).filename; } std::string Sp3Loader::UrlForEpoch(Real epoch, Time time_scale) { return BuildSp3ProductInfo(epoch, time_scale).url; } std::filesystem::path Sp3Loader::DownloadFileForEpoch(Real epoch, Time time_scale, const std::filesystem::path& cache_dir) { const Sp3ProductInfo info = BuildSp3ProductInfo(epoch, time_scale); const std::filesystem::path sp3_dir = cache_dir.empty() ? (GetOutputDir("gnss_files") / "sp3") : cache_dir; const std::filesystem::path sp3_path = sp3_dir / info.filename; if (std::filesystem::exists(sp3_path)) return sp3_path; const std::filesystem::path gzip_path = sp3_dir / info.zipped_filename; if (!std::filesystem::exists(gzip_path)) { LUPNT_CHECK(DownloadToFileEarthdata(info.url, gzip_path), "Failed to download SP3 file from CDDIS. Configure Earthdata credentials with " "~/.netrc or EARTHDATA_USERNAME/EARTHDATA_PASSWORD, then retry. URL: " + info.url, "Sp3Loader"); } if (LooksLikeHtml(gzip_path)) { std::error_code ec; std::filesystem::remove(gzip_path, ec); LUPNT_CHECK(false, "CDDIS returned an Earthdata Login HTML page instead of the requested SP3 file. " "Configure Earthdata credentials with ~/.netrc or EARTHDATA_USERNAME/" "EARTHDATA_PASSWORD, then retry. URL: " + info.url, "Sp3Loader"); } GunzipToFile(gzip_path, sp3_path); std::error_code ec; std::filesystem::remove(gzip_path, ec); return sp3_path; } // Loading ******************************************************************** void Sp3Loader::LoadFile(const std::filesystem::path& filepath) { ParseFile(filepath); // (Re-)build sorted, de-duplicated per-satellite epoch/position tables. // Mirrors the sort + `np.unique` de-duplication in `SP3Loader.get_posvelclock`. sats_.clear(); for (const auto& [sat, _] : epochs_tai_) sats_.push_back(sat); std::sort(sats_.begin(), sats_.end()); for (const auto& sat : sats_) { VecXd& epochs = epochs_tai_.at(sat); MatXd& pc = pos_clock_.at(sat); std::vector order(epochs.size()); std::iota(order.begin(), order.end(), 0); std::sort(order.begin(), order.end(), [&epochs](int a, int b) { return epochs(a) < epochs(b); }); VecXd epochs_sorted(epochs.size()); MatXd pc_sorted(pc.rows(), pc.cols()); for (int i = 0; i < static_cast(order.size()); i++) { epochs_sorted(i) = epochs(order[i]); pc_sorted.row(i) = pc.row(order[i]); } // De-duplicate consecutive equal epochs (keep first occurrence, matching // `np.unique(..., return_index=True)` on sorted data). std::vector keep; keep.reserve(epochs_sorted.size()); for (int i = 0; i < epochs_sorted.size(); i++) { if (i == 0 || epochs_sorted(i) != epochs_sorted(i - 1)) keep.push_back(i); } VecXd epochs_unique(keep.size()); MatXd pc_unique(keep.size(), pc.cols()); for (size_t i = 0; i < keep.size(); i++) { epochs_unique(i) = epochs_sorted(keep[i]); pc_unique.row(i) = pc_sorted.row(keep[i]); } epochs = epochs_unique; pc = pc_unique; } RebuildChebyshevModels(); } void Sp3Loader::ParseFile(const std::filesystem::path& filepath) { LUPNT_CHECK(std::filesystem::exists(filepath), "SP3 file not found: " + filepath.string(), "Sp3Loader"); std::ifstream file = OpenFile(filepath); // Accumulate raw samples per-satellite, then concatenate with any // previously-loaded data before sorting (done in `LoadFile`). std::map> new_epochs; std::map>> new_pos_clock; std::string line; Real current_epoch = 0.0; bool have_epoch = false; while (std::getline(file, line)) { if (line.empty()) continue; if (line[0] == '*') { // Epoch line: "* yyyy mm dd hh mm ss.sssss" std::istringstream ss(line.substr(1)); int year, month, day, hour, minute; double second; ss >> year >> month >> day >> hour >> minute >> second; current_epoch = ConvertTime(GregorianToTime(year, month, day, hour, minute, second), Time::GPS, Time::TAI); have_epoch = true; } else if (line[0] == 'P' && have_epoch) { // Position line: "P{sv}{x:14.6f}{y:14.6f}{z:14.6f}{clock:14.6f}" (fixed columns) if (line.size() < 60) continue; std::string sv = line.substr(1, 3); sv.erase(sv.find_last_not_of(" \t") + 1); sv.erase(0, sv.find_first_not_of(" \t")); if (sv.empty()) continue; double x = std::stod(line.substr(4, 14)) * 1000.0; // km -> m double y = std::stod(line.substr(18, 14)) * 1000.0; // km -> m double z = std::stod(line.substr(32, 14)) * 1000.0; // km -> m double clock = std::stod(line.substr(46, 14)) * 1e-6; // microseconds -> s new_epochs[sv].push_back(current_epoch.val()); new_pos_clock[sv].push_back({x, y, z, clock}); } } for (auto& [sat, ep_vec] : new_epochs) { const auto& pc_vec = new_pos_clock.at(sat); int n_new = static_cast(ep_vec.size()); auto it_ep = epochs_tai_.find(sat); if (it_ep == epochs_tai_.end()) { VecXd epochs(n_new); MatXd pc(n_new, 4); for (int i = 0; i < n_new; i++) { epochs(i) = ep_vec[i]; for (int k = 0; k < 4; k++) pc(i, k) = pc_vec[i][k]; } epochs_tai_[sat] = epochs; pos_clock_[sat] = pc; } else { VecXd& epochs = it_ep->second; MatXd& pc = pos_clock_.at(sat); int n_old = static_cast(epochs.size()); VecXd epochs_cat(n_old + n_new); MatXd pc_cat(n_old + n_new, 4); epochs_cat.head(n_old) = epochs; pc_cat.topRows(n_old) = pc; for (int i = 0; i < n_new; i++) { epochs_cat(n_old + i) = ep_vec[i]; for (int k = 0; k < 4; k++) pc_cat(n_old + i, k) = pc_vec[i][k]; } epochs = epochs_cat; pc = pc_cat; } } } void Sp3Loader::RebuildChebyshevModels() { pos_clock_cheby_.clear(); for (const auto& sat : sats_) { const VecXd& epochs = epochs_tai_.at(sat); const MatXd& pc = pos_clock_.at(sat); if (epochs.size() < 2) continue; double min_step = epochs(epochs.size() - 1) - epochs(0); for (int i = 1; i < epochs.size(); ++i) { if (epochs(i) > epochs(i - 1)) min_step = std::min(min_step, epochs(i) - epochs(i - 1)); } const double segment_length = std::max(1.0, 8.0 * min_step); const int num_coeffs = std::min(12, std::max(2, static_cast(epochs.size()))); pos_clock_cheby_[sat] = FitChebyshevModel( [&epochs, &pc](double t) { VecXd sample(4); for (int k = 0; k < 4; ++k) { VecXd col = pc.col(k); sample(k) = LinearInterp1d(epochs, col, t); } return sample; }, epochs(0), epochs(epochs.size() - 1), 4, segment_length, num_coeffs); } } // Queries ******************************************************************** bool Sp3Loader::HasSatellite(const std::string& sat_id) const { return epochs_tai_.find(sat_id) != epochs_tai_.end(); } std::pair Sp3Loader::GetTimeSpan(const std::string& sat_id) const { auto it = epochs_tai_.find(sat_id); LUPNT_CHECK(it != epochs_tai_.end(), "Satellite '" + sat_id + "' not found in SP3 ephemeris", "Sp3Loader"); const VecXd& epochs = it->second; return {epochs(0), epochs(epochs.size() - 1)}; } void Sp3Loader::GetPosVelClock(const std::string& sat_id, Real t_tai, Vec6& rv_ecef, Real& clock_bias_s) const { auto it_ep = epochs_tai_.find(sat_id); LUPNT_CHECK(it_ep != epochs_tai_.end(), "Satellite '" + sat_id + "' not found in SP3 ephemeris", "Sp3Loader"); const VecXd& epochs = it_ep->second; LUPNT_CHECK(epochs.size() >= 2, "SP3 ephemeris for satellite '" + sat_id + "' has fewer than 2 samples", "Sp3Loader"); double t = t_tai.val(); LUPNT_CHECK(t >= epochs(0) && t <= epochs(epochs.size() - 1), "Requested epoch is outside the loaded SP3 ephemeris span for satellite '" + sat_id + "' (no orbital-propagation fallback)", "Sp3Loader"); auto it_model = pos_clock_cheby_.find(sat_id); LUPNT_CHECK( it_model != pos_clock_cheby_.end(), "Chebyshev SP3 interpolation model for satellite '" + sat_id + "' has not been built", "Sp3Loader"); VecX pos_clock; VecX pos_clock_dot; LUPNT_CHECK(it_model->second.Eval(t_tai, &pos_clock, &pos_clock_dot), "Requested epoch is outside the Chebyshev SP3 interpolation span for satellite '" + sat_id + "'", "Sp3Loader"); rv_ecef.head(3) = pos_clock.head(3); rv_ecef.tail(3) = pos_clock_dot.head(3); clock_bias_s = pos_clock(3); // Relativistic clock correction: t_corr_rel = -2/c^2 * dot(r, v) Real dot_rv = rv_ecef.head(3).dot(rv_ecef.tail(3)); Real t_corr_rel = -2.0 / (kC * kC) * dot_rv; clock_bias_s = clock_bias_s + t_corr_rel; } Vec6 Sp3Loader::GetPosVel(const std::string& sat_id, Real t_tai) const { Vec6 rv_ecef; Real clock_bias_s; GetPosVelClock(sat_id, t_tai, rv_ecef, clock_bias_s); return rv_ecef; } } // namespace lupnt