GnssObs.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 <limits>
#include <optional>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
 
#include "NodeData/NodeData.hpp"
 
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
#include "Navigation/GNSS/Core/Code.hpp"
#include "util/Assert.h"
 
namespace NAV
{
class GnssObs : public NodeData
{
  public:
    enum ObservationType
    {
        Pseudorange,           
        Carrier,               
        Doppler,               
        ObservationType_COUNT, 
    };
 
    struct ObservationData
    {
        struct Pseudorange
        {
            double value = 0.0;
 
            uint8_t SSI = 0;
        };
 
        struct CarrierPhase
        {
            double value = 0.0;
 
            uint8_t SSI = 0;
 
            uint8_t LLI = 0;
        };
 
        explicit ObservationData(const SatSigId& satSigId) : satSigId(satSigId) {}
 
#ifdef TESTING
        ObservationData(const SatSigId& satSigId,
                        std::optional<Pseudorange> pseudorange,
                        std::optional<CarrierPhase> carrierPhase,
                        std::optional<double> doppler,
                        std::optional<double> CN0)
            : satSigId(satSigId),
              pseudorange(pseudorange),
              carrierPhase(carrierPhase),
              doppler(doppler),
              CN0(CN0)
        {}
#endif
 
        SatSigId satSigId = { Code::None, 0 };    
        std::optional<Pseudorange> pseudorange;   
        std::optional<CarrierPhase> carrierPhase; 
        std::optional<double> doppler;            
        std::optional<double> CN0;                
    };
 
    struct SatelliteData
    {
        SatId satId;                       
        Frequency frequencies = Freq_None; 
    };
 
#ifdef TESTING
    GnssObs() = default;
 
    GnssObs(const InsTime& insTime, std::vector<ObservationData> data, std::vector<SatelliteData> satData)
        : data(std::move(data)), _satData(std::move(satData))
    {
        this->insTime = insTime;
    }
#endif
    [[nodiscard]] static std::string type()
    {
        return "GnssObs";
    }
 
    [[nodiscard]] static std::vector<std::string> parentTypes()
    {
        return { NodeData::type() };
    }
 
    std::vector<ObservationData> data;
 
    SatelliteData& satData(const SatId& satId)
    {
        auto iter = std::find_if(_satData.begin(), _satData.end(), [&satId](const SatelliteData& sat) {
            return sat.satId == satId;
        });
        if (iter != _satData.end())
        {
            return *iter;
        }
 
        _satData.emplace_back();
        _satData.back().satId = satId;
        return _satData.back();
    }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const SatelliteData>> satData(const SatId& satId) const
    {
        auto iter = std::find_if(_satData.begin(), _satData.end(), [&satId](const SatelliteData& sat) {
            return sat.satId == satId;
        });
        if (iter != _satData.end())
        {
            return *iter;
        }
        return std::nullopt;
    }
 
    [[nodiscard]] bool contains(const SatSigId& satSigId) const
    {
        auto iter = std::find_if(data.begin(), data.end(), [&satSigId](const ObservationData& idData) {
            return idData.satSigId == satSigId;
        });
        return iter != data.end();
    }
 
    ObservationData& operator()(const SatSigId& satSigId)
    {
        auto iter = std::find_if(data.begin(), data.end(), [&satSigId](const ObservationData& idData) {
            return idData.satSigId == satSigId;
        });
        if (iter != data.end())
        {
            return *iter;
        }
 
        data.emplace_back(satSigId);
        return data.back();
    }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const ObservationData>> operator()(const SatSigId& satSigId) const
    {
        auto iter = std::find_if(data.begin(), data.end(), [&satSigId](const ObservationData& idData) {
            return idData.satSigId == satSigId;
        });
 
        if (iter != data.end())
        {
            return *iter;
        }
        return std::nullopt;
    }
 
    [[nodiscard]] const std::vector<SatelliteData>& getSatData() const { return _satData; }
 
    [[nodiscard]] std::vector<std::string> dynamicDataDescriptors() const override
    {
        std::vector<std::string> descriptors;
        descriptors.reserve(data.size() * 7);
 
        for (const auto& obsData : data)
        {
            descriptors.push_back(fmt::format("{} Pseudorange [m]", obsData.satSigId));
            descriptors.push_back(fmt::format("{} Pseudorange SSI", obsData.satSigId));
 
            descriptors.push_back(fmt::format("{} Carrier-phase [cycles]", obsData.satSigId));
            descriptors.push_back(fmt::format("{} Carrier-phase SSI", obsData.satSigId));
            descriptors.push_back(fmt::format("{} Carrier-phase LLI", obsData.satSigId));
 
            descriptors.push_back(fmt::format("{} Doppler [Hz]", obsData.satSigId));
            descriptors.push_back(fmt::format("{} Carrier-to-Noise density [dBHz]", obsData.satSigId));
        }
 
        return descriptors;
    }
 
    [[nodiscard]] std::optional<double> getDynamicDataAt(const std::string& descriptor) const override
    {
        for (const auto& obsData : data)
        {
            if (descriptor == fmt::format("{} Pseudorange [m]", obsData.satSigId) && obsData.pseudorange)
            {
                return obsData.pseudorange->value;
            }
            if (descriptor == fmt::format("{} Pseudorange SSI", obsData.satSigId) && obsData.pseudorange)
            {
                return obsData.pseudorange->SSI;
            }
            if (descriptor == fmt::format("{} Carrier-phase [cycles]", obsData.satSigId) && obsData.carrierPhase)
            {
                return obsData.carrierPhase->value;
            }
            if (descriptor == fmt::format("{} Carrier-phase SSI", obsData.satSigId) && obsData.carrierPhase)
            {
                return obsData.carrierPhase->SSI;
            }
            if (descriptor == fmt::format("{} Carrier-phase LLI", obsData.satSigId) && obsData.carrierPhase)
            {
                return obsData.carrierPhase->LLI;
            }
            if (descriptor == fmt::format("{} Doppler [Hz]", obsData.satSigId) && obsData.doppler)
            {
                return obsData.doppler.value();
            }
            if (descriptor == fmt::format("{} Carrier-to-Noise density [dBHz]", obsData.satSigId) && obsData.CN0)
            {
                return obsData.CN0.value();
            }
        }
        return std::nullopt;
    }
 
    [[nodiscard]] std::vector<std::pair<std::string, double>> getDynamicData() const override
    {
        std::vector<std::pair<std::string, double>> dynData;
        dynData.reserve(data.size() * 7);
        for (const auto& obsData : data)
        {
            if (obsData.pseudorange) { dynData.emplace_back(fmt::format("{} Pseudorange [m]", obsData.satSigId), obsData.pseudorange->value); }
            if (obsData.pseudorange) { dynData.emplace_back(fmt::format("{} Pseudorange SSI", obsData.satSigId), obsData.pseudorange->SSI); }
 
            if (obsData.carrierPhase) { dynData.emplace_back(fmt::format("{} Carrier-phase [cycles]", obsData.satSigId), obsData.carrierPhase->value); }
            if (obsData.carrierPhase) { dynData.emplace_back(fmt::format("{} Carrier-phase SSI", obsData.satSigId), obsData.carrierPhase->SSI); }
            if (obsData.carrierPhase) { dynData.emplace_back(fmt::format("{} Carrier-phase LLI", obsData.satSigId), obsData.carrierPhase->LLI); }
 
            if (obsData.doppler) { dynData.emplace_back(fmt::format("{} Doppler [Hz]", obsData.satSigId), obsData.doppler.value()); }
 
            if (obsData.CN0) { dynData.emplace_back(fmt::format("{} Carrier-to-Noise density [dBHz]", obsData.satSigId), obsData.CN0.value()); }
        }
        return dynData;
    }
 
    struct ReceiverInfo
    {
        std::optional<Eigen::Vector3d> e_approxPos;
 
        std::string antennaType;
 
        Eigen::Vector3d antennaDeltaNEU = Eigen::Vector3d::Zero();
    };
 
    std::optional<std::reference_wrapper<ReceiverInfo>> receiverInfo;
 
  private:
    std::vector<SatelliteData> _satData;
};
 
constexpr const char* to_string(GnssObs::ObservationType obsType)
{
    switch (obsType)
    {
    case GnssObs::Pseudorange:
        return "Pseudorange";
    case GnssObs::Carrier:
        return "Carrier";
    case GnssObs::Doppler:
        return "Doppler";
    case GnssObs::ObservationType_COUNT:
        return "COUNT";
    }
    return "";
}
 
} // namespace NAV
 
#ifndef DOXYGEN_IGNORE
 
template<>
struct fmt::formatter<NAV::GnssObs::ObservationType> : fmt::formatter<const char*>
{
    template<typename FormatContext>
    auto format(const NAV::GnssObs::ObservationType& obsType, FormatContext& ctx)
    {
        return fmt::formatter<const char*>::format(to_string(obsType), ctx);
    }
};
 
#endif
 
std::ostream& operator<<(std::ostream& os, const NAV::GnssObs::ObservationType& obj);