ReceiverClock.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 <algorithm>
#include <array>
#include <cstddef>
#include <cstdint>
#include <optional>
#include <vector>
 
#include "util/Container/UncertainValue.hpp"
#include "Navigation/GNSS/Core/SatelliteSystem.hpp"
#include "util/Logger.hpp"
 
namespace NAV
{
 
struct ReceiverClock
{
    explicit ReceiverClock(std::vector<SatelliteSystem> satelliteSystems)
        : satelliteSystems(std::move(satelliteSystems))
    {
        bias.resize(this->satelliteSystems.size());
        biasStdDev.resize(this->satelliteSystems.size());
        drift.resize(this->satelliteSystems.size());
        driftStdDev.resize(this->satelliteSystems.size());
    }
 
    void addSystem(SatelliteSystem satSys)
    {
        if (std::ranges::find(satelliteSystems, satSys) != satelliteSystems.end())
        {
            return;
        }
        satelliteSystems.push_back(satSys);
        bias.emplace_back();
        biasStdDev.emplace_back();
        drift.emplace_back();
        driftStdDev.emplace_back();
    }
 
    void clear()
    {
        satelliteSystems.clear();
        bias.clear();
        biasStdDev.clear();
        drift.clear();
        driftStdDev.clear();
    }
 
    void reset()
    {
        std::ranges::for_each(bias, [](double& v) { v = 0; });
        std::ranges::for_each(biasStdDev, [](double& v) { v = 0; });
        std::ranges::for_each(drift, [](double& v) { v = 0; });
        std::ranges::for_each(driftStdDev, [](double& v) { v = 0; });
    }
 
    [[nodiscard]] const double* biasFor(SatelliteSystem satSys) const
    {
        if (auto i = getIdx(satSys)) { return &bias.at(*i); }
        return nullptr;
    }
    [[nodiscard]] double* biasFor(SatelliteSystem satSys)
    {
        if (auto i = getIdx(satSys)) { return &bias.at(*i); }
        return nullptr;
    }
 
    [[nodiscard]] const double* biasStdDevFor(SatelliteSystem satSys) const
    {
        if (auto i = getIdx(satSys)) { return &biasStdDev.at(*i); }
        return nullptr;
    }
    [[nodiscard]] double* biasStdDevFor(SatelliteSystem satSys)
    {
        if (auto i = getIdx(satSys)) { return &biasStdDev.at(*i); }
        return nullptr;
    }
 
    [[nodiscard]] const double* driftFor(SatelliteSystem satSys) const
    {
        if (auto i = getIdx(satSys)) { return &drift.at(*i); }
        return nullptr;
    }
    [[nodiscard]] double* driftFor(SatelliteSystem satSys)
    {
        if (auto i = getIdx(satSys)) { return &drift.at(*i); }
        return nullptr;
    }
 
    [[nodiscard]] const double* driftStdDevFor(SatelliteSystem satSys) const
    {
        if (auto i = getIdx(satSys)) { return &driftStdDev.at(*i); }
        return nullptr;
    }
    [[nodiscard]] double* driftStdDevFor(SatelliteSystem satSys)
    {
        if (auto i = getIdx(satSys)) { return &driftStdDev.at(*i); }
        return nullptr;
    }
 
    [[nodiscard]] std::optional<size_t> getIdx(SatelliteSystem satSys) const
    {
        for (size_t i = 0; i < satelliteSystems.size(); i++)
        {
            if (satelliteSystems.at(i) == satSys) { return i; }
        }
        return std::nullopt;
    }
 
    std::vector<SatelliteSystem> satelliteSystems;
 
    std::vector<double> bias;
    std::vector<double> biasStdDev;
    std::vector<double> drift;
    std::vector<double> driftStdDev;
};
 
} // namespace NAV