gnss.hpp

// Copyright (C) 2024 Avular Holding B.V. - All Rights Reserved
// You may use this code under the terms of the Avular
// Software End-User License Agreement.
//
// You should have received a copy of the Avular
// Software End-User License Agreement license with
// this file, or download it from: avular.com/eula
//
/*****************************************************************************
 * Created on: 2024 July 08
 * Author: Bram Tertoolen
 *
 * @file Message containing GNSS data of the robot
 ****************************************************************************/
#pragma once
 
#include <array>
#include <creos/robot_clock.hpp>
#include <nlohmann/json.hpp>
#include <string>
 
#include "generic.hpp"
 
namespace creos_messages {
 
struct GnssCoordinate {
    double latitude = 0.0;
    double longitude = 0.0;
    double altitude = 0.0;  // [m] NaN if no altitude is available
 
    auto operator<=>(const GnssCoordinate& other) const = default;
};
struct GnssCoordinate {…};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(GnssCoordinate, latitude, longitude, altitude)
 
 
struct GnssPosition {
    enum CovarianceType {
        kInvalidCovariance = -1,  
        kUnknown = 0,             
        kApproximated = 1,        
        kDiagonalKnown = 2,       
        kKnown = 3                
    };
    enum CovarianceType {…};
 
    GnssCoordinate coordinate;
    Matrixd<3, 3> covariance;
    CovarianceType position_covariance_type = kInvalidCovariance;
 
    auto operator<=>(const GnssPosition& other) const = default;
};
struct GnssPosition {…};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(GnssPosition, coordinate, covariance, position_covariance_type)
 
 
struct GnssHeading {
 
    double heading = 0.0;             
    double heading_covariance = 0.0;  
 
    auto operator<=>(const GnssHeading& other) const = default;
};
struct GnssHeading {…};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(GnssHeading, heading, heading_covariance)
 
 
struct Gnss {
 
    struct Service {
        bool gps : 1 = false;      
        bool glonass : 1 = false;  
        bool beidou : 1 = false;   
        bool galileo : 1 = false;  
 
        auto operator<=>(const Service& other) const = default;
    };
    struct Service {…};
 
    enum Augmentation {
        kInvalidStatus = -2,              
        kNoAugmentationFix = -1,          
        kUnaugmentedFix = 0,              
        kSatelliteBasedAugmentation = 1,  
        kGroundBasedAugmentation = 2      
    };
    enum Augmentation {…};
 
    enum FixStatus {
        kNoFix = -1,    
        k2DFix = 0,     
        k3DFix = 1,     
        kRTKFloat = 2,  
        kRTKFix = 3,    
    };
    enum FixStatus {…};
 
    creos::RobotClock::time_point timestamp;
 
    std::string frame_id;
 
    Augmentation augmentation = Augmentation::kInvalidStatus;
 
    Service service;
 
    GnssPosition position;
 
    GnssHeading heading;
 
    FixStatus fix_status = FixStatus::kNoFix;
 
    int satellites_visible = 0;
 
    int satellites_used = 0;
 
    auto operator<=>(const Gnss& other) const = default;
};
struct Gnss {…};
 
namespace detail {
enum class GnssServiceFlags : uint32_t {
    kNone = 0x0,
    kGps = 0x1,
    kGlonass = 0x2,
    kBeidou = 0x4,
    kGalileo = 0x8,
};
 
inline constexpr bool isSet(uint32_t flags, GnssServiceFlags flag) {
    return (static_cast<uint32_t>(flag) & flags) != 0;
}
 
inline constexpr void addFlag(uint32_t& flags, GnssServiceFlags flag, bool value) {
    if (value) {
        flags |= static_cast<uint32_t>(flag);
    }
}
 
}  // namespace detail
 
inline void to_json(nlohmann::json& j, const Gnss::Service& service) {
    using namespace detail;
 
    uint32_t flags = 0;
    addFlag(flags, GnssServiceFlags::kGps, service.gps);
    addFlag(flags, GnssServiceFlags::kGlonass, service.glonass);
    addFlag(flags, GnssServiceFlags::kBeidou, service.beidou);
    addFlag(flags, GnssServiceFlags::kGalileo, service.galileo);
 
    j = flags;
}
 
inline void from_json(const nlohmann::json& j, Gnss::Service& service) {
    using namespace detail;
 
    uint32_t flags = j.get<uint32_t>();
 
    service.gps = isSet(flags, GnssServiceFlags::kGps);
    service.glonass = isSet(flags, GnssServiceFlags::kGlonass);
    service.beidou = isSet(flags, GnssServiceFlags::kBeidou);
    service.galileo = isSet(flags, GnssServiceFlags::kGalileo);
}
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Gnss, timestamp, frame_id, augmentation, service, position, heading, fix_status,
                                   satellites_visible, satellites_used)
 
}  // namespace creos_messages