Keys.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 <vector>
#include <variant>
#include <fmt/format.h>
 
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
 
namespace NAV::SPP
{
 
namespace States
{
enum SppStates
{
    PosX,            
    PosY,            
    PosZ,            
    VelX,            
    VelY,            
    VelZ,            
    RecvClkErr,      
    RecvClkDrift,    
    SppStates_COUNT, 
};
 
constexpr size_t POS_STATE_COUNT = 4; 
constexpr size_t VEL_STATE_COUNT = 4; 
 
constexpr size_t POS_VEL_STATE_COUNT = 6; 
 
struct InterSysBias
{
    bool operator==(const InterSysBias& rhs) const { return satSys == rhs.satSys; }
    SatelliteSystem satSys;
};
struct InterSysDrift
{
    bool operator==(const InterSysDrift& rhs) const { return satSys == rhs.satSys; }
    SatelliteSystem satSys;
};
struct InterFreqBias
{
    bool operator==(const InterFreqBias& rhs) const { return freq == rhs.freq; }
    Frequency freq;
};
 
using StateKeyTypes = std::variant<SppStates, InterSysBias, InterSysDrift, InterFreqBias>;
inline static const std::vector<StateKeyTypes> Pos = { SppStates::PosX, SppStates::PosY, SppStates::PosZ };
inline static const std::vector<StateKeyTypes> Vel = { SppStates::VelX, SppStates::VelY, SppStates::VelZ };
inline static const std::vector<StateKeyTypes> RecvClk = { SppStates::RecvClkErr, SppStates::RecvClkDrift };
inline static const std::vector<StateKeyTypes> PosVel = { SppStates::PosX, SppStates::PosY, SppStates::PosZ,
                                                          SppStates::VelX, SppStates::VelY, SppStates::VelZ };
inline static const std::vector<StateKeyTypes> PosVelRecvClkErr = { SppStates::PosX, SppStates::PosY, SppStates::PosZ,
                                                                    SppStates::VelX, SppStates::VelY, SppStates::VelZ,
                                                                    SppStates::RecvClkErr };
inline static const std::vector<StateKeyTypes> PosVelRecvClk = { SppStates::PosX, SppStates::PosY, SppStates::PosZ,
                                                                 SppStates::VelX, SppStates::VelY, SppStates::VelZ,
                                                                 SppStates::RecvClkErr, SppStates::RecvClkDrift };
 
inline static const std::vector<StateKeyTypes> PosRecvClkErr = { SppStates::PosX, SppStates::PosY, SppStates::PosZ,
                                                                 SppStates::RecvClkErr };
inline static const std::vector<StateKeyTypes> VelRecvClkDrift = { SppStates::VelX, SppStates::VelY, SppStates::VelZ,
                                                                   SppStates::RecvClkDrift };
 
} // namespace States
 
namespace Meas
{
 
struct Psr
{
    bool operator==(const Psr& rhs) const { return satSigId == rhs.satSigId; }
    SatSigId satSigId;
};
struct Doppler
{
    bool operator==(const Doppler& rhs) const { return satSigId == rhs.satSigId; }
    SatSigId satSigId;
};
 
using MeasKeyTypes = std::variant<Psr, Doppler>;
 
} // namespace Meas
 
} // namespace NAV::SPP
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::States::SppStates& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::States::InterSysBias& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::States::InterSysDrift& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::States::InterFreqBias& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::Meas::Psr& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::Meas::Doppler& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::States::StateKeyTypes& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::SPP::Meas::MeasKeyTypes& obj);
 
namespace std
{
template<>
struct hash<NAV::SPP::States::InterSysBias>
{
    size_t operator()(const NAV::SPP::States::InterSysBias& interSysBias) const
    {
        return NAV::SPP::States::SppStates_COUNT + std::hash<NAV::SatelliteSystem>()(interSysBias.satSys);
    }
};
template<>
struct hash<NAV::SPP::States::InterSysDrift>
{
    size_t operator()(const NAV::SPP::States::InterSysDrift& interSysDrift) const
    {
        return NAV::SPP::States::SppStates_COUNT + std::hash<NAV::SatelliteSystem>()(interSysDrift.satSys);
    }
};
template<>
struct hash<NAV::SPP::States::InterFreqBias>
{
    size_t operator()(const NAV::SPP::States::InterFreqBias& interFreqBias) const
    {
        return NAV::SPP::States::SppStates_COUNT + std::hash<NAV::Frequency>()(interFreqBias.freq);
    }
};
template<>
struct hash<NAV::SPP::Meas::Psr>
{
    size_t operator()(const NAV::SPP::Meas::Psr& psr) const
    {
        return std::hash<NAV::SatSigId>()(psr.satSigId);
    }
};
template<>
struct hash<NAV::SPP::Meas::Doppler>
{
    size_t operator()(const NAV::SPP::Meas::Doppler& doppler) const
    {
        return std::hash<NAV::SatSigId>()(doppler.satSigId) << 12;
    }
};
} // namespace std
 
#ifndef DOXYGEN_IGNORE
 
template<>
struct fmt::formatter<NAV::SPP::States::SppStates> : fmt::formatter<const char*>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::States::SppStates& state, FormatContext& ctx)
    {
        using namespace NAV::SPP::States; // NOLINT(google-build-using-namespace)
 
        switch (state)
        {
        case PosX:
            return fmt::formatter<const char*>::format("PosX", ctx);
        case PosY:
            return fmt::formatter<const char*>::format("PosY", ctx);
        case PosZ:
            return fmt::formatter<const char*>::format("PosZ", ctx);
        case VelX:
            return fmt::formatter<const char*>::format("VelX", ctx);
        case VelY:
            return fmt::formatter<const char*>::format("VelY", ctx);
        case VelZ:
            return fmt::formatter<const char*>::format("VelZ", ctx);
        case RecvClkErr:
            return fmt::formatter<const char*>::format("RecvClkErr", ctx);
        case RecvClkDrift:
            return fmt::formatter<const char*>::format("RecvClkDrift", ctx);
        case SppStates_COUNT:
            return fmt::formatter<const char*>::format("SppStates_COUNT", ctx);
        }
 
        return fmt::formatter<const char*>::format("ERROR", ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::States::InterSysBias> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::States::InterSysBias& interSysBias, FormatContext& ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("InterSysBias({})", interSysBias.satSys), ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::States::InterSysDrift> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::States::InterSysDrift& interSysDrift, FormatContext& ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("InterSysDrift({})", interSysDrift.satSys), ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::States::InterFreqBias> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::States::InterFreqBias& interFreqBias, FormatContext& ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("InterFreqBias({})", interFreqBias.freq), ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::Meas::Psr> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::Meas::Psr& psr, FormatContext& ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("psr({})", psr.satSigId), ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::Meas::Doppler> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::Meas::Doppler& doppler, FormatContext& ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("dop({})", doppler.satSigId), ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::States::StateKeyTypes> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::States::StateKeyTypes& state, FormatContext& ctx)
    {
        using namespace NAV::SPP::States; // NOLINT(google-build-using-namespace)
 
        if (const auto* s = std::get_if<NAV::SPP::States::SppStates>(&state))
        {
            switch (*s)
            {
            case PosX:
                return fmt::formatter<std::string>::format("PosX", ctx);
            case PosY:
                return fmt::formatter<std::string>::format("PosY", ctx);
            case PosZ:
                return fmt::formatter<std::string>::format("PosZ", ctx);
            case VelX:
                return fmt::formatter<std::string>::format("VelX", ctx);
            case VelY:
                return fmt::formatter<std::string>::format("VelY", ctx);
            case VelZ:
                return fmt::formatter<std::string>::format("VelZ", ctx);
            case RecvClkErr:
                return fmt::formatter<std::string>::format("RecvClkErr", ctx);
            case RecvClkDrift:
                return fmt::formatter<std::string>::format("RecvClkDrift", ctx);
            case SppStates_COUNT:
                return fmt::formatter<std::string>::format("SppStates_COUNT", ctx);
            }
        }
        if (const auto* interSysBias = std::get_if<NAV::SPP::States::InterSysBias>(&state))
        {
            return fmt::formatter<std::string>::format(fmt::format("InterSysBias({}))", interSysBias->satSys), ctx);
        }
        if (const auto* interSysDrift = std::get_if<NAV::SPP::States::InterSysDrift>(&state))
        {
            return fmt::formatter<std::string>::format(fmt::format("InterSysDrift({}))", interSysDrift->satSys), ctx);
        }
        if (const auto* interFreqBias = std::get_if<NAV::SPP::States::InterFreqBias>(&state))
        {
            return fmt::formatter<std::string>::format(fmt::format("InterFreqBias({}))", interFreqBias->freq), ctx);
        }
 
        return fmt::formatter<std::string>::format("ERROR", ctx);
    }
};
 
template<>
struct fmt::formatter<NAV::SPP::Meas::MeasKeyTypes> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::Meas::MeasKeyTypes& meas, FormatContext& ctx)
    {
        if (const auto* psr = std::get_if<NAV::SPP::Meas::Psr>(&meas))
        {
            return fmt::formatter<std::string>::format(fmt::format("psr({})", psr->satSigId), ctx);
        }
        if (const auto* doppler = std::get_if<NAV::SPP::Meas::Doppler>(&meas))
        {
            return fmt::formatter<std::string>::format(fmt::format("doppler({})", doppler->satSigId), ctx);
        }
 
        return fmt::formatter<std::string>::format("ERROR", ctx);
    }
};
 
#endif