VectorNavTypes.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 <cstdint>
#include <vector>
#include <iostream>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <vn/vector.h>
#include <vn/matrix.h>
#include <vn/types.h>
 
namespace NAV::vendor::vectornav
{
class TimeStatus
{
  public:
    explicit TimeStatus(uint8_t status) : _status(status) {}
 
    TimeStatus& operator=(const uint8_t& status)
    {
        _status = status;
        return *this;
    }
 
    TimeStatus() = default;
 
    [[nodiscard]] constexpr uint8_t& status()
    {
        return _status;
    }
 
    [[nodiscard]] constexpr uint8_t timeOk() const
    {
        return ((_status & (1U << 0U)) >> 0U);
    }
    [[nodiscard]] constexpr uint8_t dateOk() const
    {
        return ((_status & (1U << 1U)) >> 1U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t utcTimeValid() const
    {
        return ((_status & (1U << 2U)) >> 2U); // NOLINT
    }
 
  private:
    uint8_t _status;
};
 
struct UTC
{
    int8_t year{};   
    uint8_t month{}; 
    uint8_t day{};   
    uint8_t hour{};  
    uint8_t min{};   
    uint8_t sec{};   
    uint16_t ms{};   
};
 
enum GnssFix : uint8_t
{
    GnssFix_NoFix,     
    GnssFix_TimeOnly,  
    GnssFix_2D,        
    GnssFix_3D,        
    GnssFix_SBAS,      
    GnssFix_RTK_Float, 
    GnssFix_RTK_Fixed  
};
 
struct TimeInfo
{
    TimeStatus status{};
    int8_t leapSeconds{};
};
 
struct DOP
{
    float gDop{}; 
    float pDop{}; 
    float tDop{}; 
    float vDop{}; 
    float hDop{}; 
    float nDop{}; 
    float eDop{}; 
};
 
enum class SatSys : uint8_t
{
    GPS = 0,     
    SBAS = 1,    
    Galileo = 2, 
    BeiDou = 3,  
    IMES = 4,    
    QZSS = 5,    
    GLONASS = 6, 
};
 
std::ostream& operator<<(std::ostream& os, const SatSys& satSys);
 
struct SatInfo
{
    struct SatInfoElement
    {
        enum class Flags : uint8_t
        {
            None = 0,                        
            Healthy = 1 << 0,                
            Almanac = 1 << 1,                
            Ephemeris = 1 << 2,              
            DifferentialCorrection = 1 << 3, 
            UsedForNavigation = 1 << 4,      
            AzimuthElevationValid = 1 << 5,  
            UsedForRTK = 1 << 6,             
        };
 
        friend Flags operator|(Flags lhs, Flags rhs)
        {
            return Flags(int(lhs) | int(rhs));
        }
 
        enum class QualityIndicator : uint8_t
        {
            NoSignal = 0,                                 
            SearchingSignal = 1,                          
            SignalAcquired = 2,                           
            SignalDetectedButUnstable = 3,                
            CodeLockedAndTimeSynchronized = 4,            
            CodeAndCarrierLockedAndTimeSynchronized1 = 5, 
            CodeAndCarrierLockedAndTimeSynchronized2 = 6, 
            CodeAndCarrierLockedAndTimeSynchronized3 = 7, 
        };
 
        SatInfoElement() = default;
 
        SatInfoElement(uint8_t sys, uint8_t svId, uint8_t flags, uint8_t cno, uint8_t qi, int8_t el, int16_t az)
            : sys(static_cast<SatSys>(sys)), svId(svId), flags(static_cast<Flags>(flags)), cno(cno), qi(static_cast<QualityIndicator>(qi)), el(el), az(az) {}
 
        SatInfoElement(uint8_t sys, uint8_t svId,
                       uint8_t healthy, uint8_t almanac, uint8_t ephemeris, uint8_t differentialCorrection, uint8_t usedForNavigation, uint8_t azimuthElevationValid, uint8_t usedForRTK,
                       uint8_t cno, uint8_t qi, int8_t el, int16_t az)
            : sys(static_cast<SatSys>(sys)), svId(svId), flags((healthy ? Flags::Healthy : Flags::None) | (almanac ? Flags::Almanac : Flags::None) | (ephemeris ? Flags::Ephemeris : Flags::None) | (differentialCorrection ? Flags::DifferentialCorrection : Flags::None) | (usedForNavigation ? Flags::UsedForNavigation : Flags::None) | (azimuthElevationValid ? Flags::AzimuthElevationValid : Flags::None) | (usedForRTK ? Flags::UsedForRTK : Flags::None)), cno(cno), qi(static_cast<QualityIndicator>(qi)), el(el), az(az) {}
 
        SatSys sys{};          
        uint8_t svId{};        
        Flags flags{};         
        uint8_t cno{};         
        QualityIndicator qi{}; 
        int8_t el{};           
        int16_t az{};          
    };
 
    uint8_t numSats{};
    std::vector<SatInfoElement> satellites;
};
 
constexpr SatInfo::SatInfoElement::Flags operator&(SatInfo::SatInfoElement::Flags lhs, SatInfo::SatInfoElement::Flags rhs)
{
    return SatInfo::SatInfoElement::Flags(int(lhs) & int(rhs));
}
 
struct RawMeas
{
    struct SatRawElement
    {
        enum class Flags : uint16_t
        {
            None = 0,                     
            Searching = 1 << 0,           
            Tracking = 1 << 1,            
            TimeValid = 1 << 2,           
            CodeLock = 1 << 3,            
            PhaseLock = 1 << 4,           
            PhaseHalfAmbiguity = 1 << 5,  
            PhaseHalfSub = 1 << 6,        
            PhaseSlip = 1 << 7,           
            PseudorangeSmoothed = 1 << 8, 
        };
 
        friend Flags operator|(Flags lhs, Flags rhs)
        {
            return Flags(int(lhs) | int(rhs));
        }
 
        enum class Chan : uint8_t
        {
            P_Code = 0,       
            CA_Code = 1,      
            SemiCodeless = 2, 
            Y_Code = 3,       
            M_Code = 4,       
            Codeless = 5,     
            A_Chan = 6,       
            B_Chan = 7,       
            I_Chan = 8,       
            Q_Chan = 9,       
            M_Chan = 10,      
            L_Chan = 11,      
            BC_Chan = 12,     
            Z_Tracking = 13,  
            ABC = 14,         
        };
 
        friend std::ostream& operator<<(std::ostream& os, const Chan& chan)
        {
            switch (chan)
            {
            case Chan::P_Code:
                os << "P-Code";
                break;
            case Chan::CA_Code:
                os << "C/A-Code or C-Chan";
                break;
            case Chan::SemiCodeless:
                os << "Semi-codeless";
                break;
            case Chan::Y_Code:
                os << "Y-Code";
                break;
            case Chan::M_Code:
                os << "M-Code";
                break;
            case Chan::Codeless:
                os << "Codeless";
                break;
            case Chan::A_Chan:
                os << "A Chan";
                break;
            case Chan::B_Chan:
                os << "B Chan";
                break;
            case Chan::I_Chan:
                os << "I Chan";
                break;
            case Chan::Q_Chan:
                os << "Q Chan";
                break;
            case Chan::M_Chan:
                os << "M Chan or D Chan";
                break;
            case Chan::L_Chan:
                os << "L Chan or P Chan";
                break;
            case Chan::BC_Chan:
                os << "B+C Chan, I+Q Chan, M+L Chan or D+P Chan";
                break;
            case Chan::Z_Tracking:
                os << "based on Z-tracking";
                break;
            case Chan::ABC:
                os << "A+B+C";
                break;
            }
            return os;
        }
 
        enum class Freq : uint8_t
        {
            RxChannel = 0, 
            L1 = 1,        
            L2 = 2,        
            L5 = 3,        
            E6 = 4,        
            E5b = 5,       
            E5a = 6,       
        };
 
        friend std::ostream& operator<<(std::ostream& os, const Freq& freq)
        {
            switch (freq)
            {
            case Freq::RxChannel:
                os << "Rx Channel";
                break;
            case Freq::L1:
                os << "L1, G1, E2-L1-E1 or B1";
                break;
            case Freq::L2:
                os << "L2 or G2";
                break;
            case Freq::L5:
                os << "L5 or E5a";
                break;
            case Freq::E6:
                os << "E6, LEX or B3";
                break;
            case Freq::E5b:
                os << "E5b or B2";
                break;
            case Freq::E5a:
                os << "E5a+b";
                break;
            }
            return os;
        }
 
        SatRawElement() = default;
 
        SatRawElement(uint8_t sys, uint8_t svId, uint8_t freq, uint8_t chan, int8_t slot, uint8_t cno, uint16_t flags, double pr, double cp, float dp)
            : sys(static_cast<SatSys>(sys)), svId(svId), freq(static_cast<Freq>(freq)), chan(static_cast<Chan>(chan)), slot(slot), cno(cno), flags(static_cast<Flags>(flags)), pr(pr), cp(cp), dp(dp) {}
 
        SatRawElement(uint8_t sys, uint8_t svId, uint8_t freq, uint8_t chan, int8_t slot, uint8_t cno,
                      uint8_t searching, uint8_t tracking, uint8_t timeValid, uint8_t codeLock, uint8_t phaseLock, uint8_t phaseHalfAmbiguity, uint8_t phaseHalfSub, uint8_t phaseSlip, uint8_t pseudorangeSmoothed,
                      double pr, double cp, double dp)
            : sys(static_cast<SatSys>(sys)), svId(svId), freq(static_cast<Freq>(freq)), chan(static_cast<Chan>(chan)), slot(slot), cno(cno), flags((searching ? Flags::Searching : Flags::None) | (tracking ? Flags::Tracking : Flags::None) | (timeValid ? Flags::TimeValid : Flags::None) | (codeLock ? Flags::CodeLock : Flags::None) | (phaseLock ? Flags::PhaseLock : Flags::None) | (phaseHalfAmbiguity ? Flags::PhaseHalfAmbiguity : Flags::None) | (phaseHalfSub ? Flags::PhaseHalfSub : Flags::None) | (phaseSlip ? Flags::PhaseSlip : Flags::None) | (pseudorangeSmoothed ? Flags::PseudorangeSmoothed : Flags::None)), pr(pr), cp(cp), dp(static_cast<float>(dp)) {}
 
        SatSys sys{};   
        uint8_t svId{}; 
        Freq freq{};    
        Chan chan{};    
        int8_t slot{};  
        uint8_t cno{};  
        Flags flags{};  
        double pr{};    
        double cp{};    
        float dp{};     
    };
 
    double tow{};
    uint16_t week{};
    uint8_t numSats{};
    std::vector<SatRawElement> satellites;
};
 
constexpr RawMeas::SatRawElement::Flags operator&(RawMeas::SatRawElement::Flags lhs, RawMeas::SatRawElement::Flags rhs)
{
    return RawMeas::SatRawElement::Flags(int(lhs) & int(rhs));
}
 
class VpeStatus
{
  public:
    explicit VpeStatus(uint16_t status) : _status(status) {}
 
    VpeStatus& operator=(const uint16_t& status)
    {
        _status = status;
        return *this;
    }
 
    VpeStatus() = default;
 
    [[nodiscard]] constexpr uint16_t& status()
    {
        return _status;
    }
 
    [[nodiscard]] constexpr uint8_t attitudeQuality() const
    {
        return ((_status & (1U << 0U | 1U << 1U)) >> 0U);
    }
    [[nodiscard]] constexpr uint8_t gyroSaturation() const
    {
        return ((_status & (1U << 2U)) >> 2U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t gyroSaturationRecovery() const
    {
        return ((_status & (1U << 3U)) >> 3U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t magDisturbance() const
    {
        return ((_status & (1U << 4U | 1U << 5U)) >> 4U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t magSaturation() const
    {
        return ((_status & (1U << 6U)) >> 6U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t accDisturbance() const
    {
        return ((_status & (1U << 7U | 1U << 8U)) >> 7U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t accSaturation() const
    {
        return ((_status & (1U << 9U)) >> 9U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t knownMagDisturbance() const
    {
        return ((_status & (1U << 11U)) >> 11U); // NOLINT
    }
    [[nodiscard]] constexpr uint8_t knownAccelDisturbance() const
    {
        return ((_status & (1U << 12U)) >> 12U); // NOLINT
    }
 
  private:
    uint16_t _status;
};
 
class InsStatus
{
  public:
    enum class Mode
    {
        NotTracking = 0,
        Aligning = 1,
        Tracking = 2,
        LossOfGNSS = 3,
    };
 
    explicit InsStatus(uint16_t status) : _status(status) {}
 
    InsStatus& operator=(const uint16_t& status)
    {
        _status = status;
        return *this;
    }
 
    InsStatus() = default;
 
    [[nodiscard]] constexpr uint16_t& status()
    {
        return _status;
    }
 
    [[nodiscard]] constexpr Mode mode() const
    {
        return static_cast<Mode>((_status & (1U << 0U | 1U << 1U)) >> 0U);
    }
    [[nodiscard]] constexpr bool gpsFix() const
    {
        return ((_status & (1U << 2U)) >> 2U); // NOLINT
    }
    [[nodiscard]] constexpr bool errorIMU() const
    {
        return ((_status & (1U << 4U)) >> 4U); // NOLINT
    }
    [[nodiscard]] constexpr bool errorMagPres() const
    {
        return ((_status & (1U << 5U)) >> 5U); // NOLINT
    }
    [[nodiscard]] constexpr bool errorGnss() const
    {
        return ((_status & (1U << 6U)) >> 6U); // NOLINT
    }
    [[nodiscard]] constexpr bool gpsHeadingIns() const
    {
        return ((_status & (1U << 8U)) >> 8U); // NOLINT
    }
    [[nodiscard]] constexpr bool gpsCompass() const
    {
        return ((_status & (1U << 9U)) >> 9U); // NOLINT
    }
 
  private:
    uint16_t _status;
};
 
} // namespace NAV::vendor::vectornav
 
#ifndef DOXYGEN_IGNORE
 
template<>
struct fmt::formatter<vn::protocol::uart::ErrorDetectionMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::AsciiAsync> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::AsyncMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::BinaryGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::CommonGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::TimeGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::ImuGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::GpsGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::AttitudeGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::InsGroup> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::SensorError> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::BootloaderError> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::SyncInMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::SyncInEdge> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::SyncOutMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::SyncOutPolarity> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::CountMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::StatusMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::ChecksumMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::ErrorMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::FilterMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::IntegrationFrame> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::CompensationMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::AccCompensationMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::EarthRateCorrection> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::GpsFix> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::GpsMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::PpsSource> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::GpsRate> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::AntPower> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::VpeEnable> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::HeadingMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::VpeMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::Scenario> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::HsiMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::HsiOutput> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::VelocityCompensationMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::MagneticMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::ExternalSensorMode> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::FoamInit> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::SensSat> : ostream_formatter
{};
template<>
struct fmt::formatter<vn::protocol::uart::InsStatus> : ostream_formatter
{};
 
template<>
struct fmt::formatter<NAV::vendor::vectornav::SatSys> : ostream_formatter
{};
template<>
struct fmt::formatter<NAV::vendor::vectornav::RawMeas::SatRawElement::Chan> : ostream_formatter
{};
template<>
struct fmt::formatter<NAV::vendor::vectornav::RawMeas::SatRawElement::Freq> : ostream_formatter
{};
 
template<size_t tdim, typename T>
struct fmt::formatter<vn::math::vec<tdim, T>> : ostream_formatter
{};
 
template<size_t m, size_t n, typename T>
struct fmt::formatter<vn::math::mat<m, n, T>> : ostream_formatter
{};
 
#endif