GnssNavInfo.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <unordered_map>
#include <utility>
 
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
#include "Navigation/GNSS/Core/SatelliteSystem.hpp"
#include "Navigation/Atmosphere/Ionosphere/IonosphericCorrections.hpp"
#include "Navigation/GNSS/Satellite/Satellite.hpp"
#include "util/Container/Pair.hpp"
#include "util/Logger.hpp"
 
namespace NAV
{
class GnssNavInfo
{
  public:
    [[nodiscard]] static std::string type()
    {
        return "GnssNavInfo";
    }
 
    [[nodiscard]] Orbit::Pos calcSatellitePos(const SatId& satId, const InsTime& transTime) const
    {
        return m_satellites.at(satId).calcSatellitePos(transTime);
    }
    [[nodiscard]] Orbit::PosVel calcSatellitePosVel(const SatId& satId, const InsTime& transTime) const
    {
        return m_satellites.at(satId).calcSatellitePosVel(transTime);
    }
    [[nodiscard]] Orbit::PosVelAccel calcSatellitePosVelAccel(const SatId& satId, const InsTime& transTime) const
    {
        return m_satellites.at(satId).calcSatellitePosVelAccel(transTime);
    }
 
    [[nodiscard]] Clock::Corrections calcSatelliteClockCorrections(const SatId& satId, const InsTime& recvTime, double dist, const Frequency& freq) const
    {
        return m_satellites.at(satId).calcClockCorrections(recvTime, dist, freq);
    }
 
    [[nodiscard]] double calcSatellitePositionVariance(const SatId& satId, const InsTime& recvTime) const
    {
        return m_satellites.at(satId).calcSatellitePositionVariance(recvTime);
    }
 
    [[nodiscard]] bool isHealthy(const SatId& satId, const InsTime& recvTime) const
    {
        return m_satellites.at(satId).isHealthy(recvTime);
    }
 
    void addSatelliteNavData(const SatId& satId, const std::shared_ptr<SatNavData>& satNavData)
    {
        m_satellites[satId].addSatNavData(satNavData);
    }
 
    [[nodiscard]] std::shared_ptr<NAV::SatNavData> searchNavigationData(const SatId& satId, const InsTime& recvTime) const
    {
        if (!m_satellites.contains(satId)) { return nullptr; }
 
        auto satNavData = m_satellites.at(satId).searchNavigationData(recvTime);
        if (satNavData == nullptr)
        {
            [[maybe_unused]] auto printNavData = [&]() {
                std::string ret;
                for (const auto& navData : m_satellites.at(satId).getNavigationData())
                {
                    ret += fmt::format("[{} - diff {:.0f}s], ", navData->refTime.toYMDHMS(GPST),
                                       static_cast<double>(std::abs((navData->refTime - recvTime).count())));
                }
                return ret.substr(0, ret.length() - 2);
            };
 
            LOG_TRACE("[{}][{}]: No navigation data found. Available data are at time: {}", satId, recvTime.toYMDHMS(GPST), printNavData());
        }
 
        return satNavData;
    }
 
    [[nodiscard]] size_t nSatellites() const
    {
        return m_satellites.size();
    }
 
    const auto& satellites() const
    {
        return m_satellites;
    }
 
    void reset()
    {
        satelliteSystems = SatSys_None;
        ionosphericCorrections.clear();
        timeSysCorr.clear();
        m_satellites.clear();
    }
 
    SatelliteSystem satelliteSystems = SatSys_None;
 
    IonosphericCorrections ionosphericCorrections;
 
    struct TimeSystemCorrections
    {
        double a0 = std::nan(""); 
        double a1 = std::nan(""); 
        // TODO: Add a2 value
    };
 
    std::unordered_map<std::pair<TimeSystem, TimeSystem>, TimeSystemCorrections> timeSysCorr;
 
  private:
    std::unordered_map<SatId, Satellite> m_satellites;
};
 
} // namespace NAV