UlogFile.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
 
// Already in 'FileReader.hpp'
// #include <string>
// #include <vector>
// #include <fstream>
 
#include <map>
#include <variant>
 
#include "Nodes/DataProvider/IMU/Imu.hpp"
#include "Nodes/DataProvider/Protocol/FileReader.hpp"
 
namespace NAV
{
class UlogFile : public Imu, public FileReader
{
  public:
    UlogFile();
    ~UlogFile() override;
    UlogFile(const UlogFile&) = delete;
    UlogFile(UlogFile&&) = delete;
    UlogFile& operator=(const UlogFile&) = delete;
    UlogFile& operator=(UlogFile&&) = delete;
 
    [[nodiscard]] static std::string typeStatic();
 
    [[nodiscard]] std::string type() const override;
 
    [[nodiscard]] static std::string category();
 
    void guiConfig() override;
 
    [[nodiscard]] json save() const override;
 
    void restore(const json& j) override;
 
    bool resetNode() override;
 
    struct SubscriptionData
    {
        uint8_t multi_id;         
        std::string message_name; 
    };
 
  private:
    constexpr static size_t OUTPUT_PORT_INDEX_IMUOBS_1 = 0;  
    constexpr static size_t OUTPUT_PORT_INDEX_IMUOBS_2 = 1;  
    constexpr static size_t OUTPUT_PORT_INDEX_POSVELATT = 2; 
 
    bool initialize() override;
 
    void deinitialize() override;
 
    [[nodiscard]] std::shared_ptr<const NodeData> pollData();
 
    [[nodiscard]] FileType determineFileType() override;
 
    void readHeader() override;
 
    void readInformationMessage(uint16_t msgSize, char msgType);
 
    void readInformationMessageMulti(uint16_t msgSize, char msgType);
 
    void readParameterMessage(uint16_t msgSize, char msgType);
 
    void readParameterMessageDefault(uint16_t msgSize, char msgType);
 
    struct DataField
    {
        std::string type; 
        std::string name; 
    };
 
    std::unordered_map<std::string, std::vector<DataField>> _messageFormats;
 
    struct SensorAccel
    {
        uint64_t timestamp;                  
        uint64_t timestamp_sample;           
        uint32_t device_id;                  
        float x;                             
        float y;                             
        float z;                             
        float temperature;                   
        uint32_t error_count;                
        std::array<uint8_t, 3> clip_counter; 
 
        static constexpr uint8_t padding = 5; 
    };
 
    struct SensorGyro
    {
        uint64_t timestamp;        
        uint64_t timestamp_sample; 
        uint32_t device_id;        
        float x;                   
        float y;                   
        float z;                   
        float temperature;         
        uint32_t error_count;      
    };
 
    struct SensorMag
    {
        uint64_t timestamp;        
        uint64_t timestamp_sample; 
        uint32_t device_id;        
        float x;                   
        float y;                   
        float z;                   
        float temperature;         
        uint32_t error_count;      
        bool is_external;          
 
        static constexpr uint8_t padding = 7; 
    };
 
    struct VehicleGpsPosition
    {
        uint64_t timestamp;              
        uint64_t time_utc_usec;          
        int32_t lat;                     
        int32_t lon;                     
        int32_t alt;                     
        int32_t alt_ellipsoid;           
        float s_variance_m_s;            
        float c_variance_rad;            
        float eph;                       
        float epv;                       
        float hdop;                      
        float vdop;                      
        int32_t noise_per_ms;            
        int32_t jamming_indicator;       
        float vel_m_s;                   
        float vel_n_m_s;                 
        float vel_e_m_s;                 
        float vel_d_m_s;                 
        float cog_rad;                   
        int32_t timestamp_time_relative; 
        float heading;                   
        float heading_offset;            
        uint8_t fix_type;                
        bool vel_ned_valid;              
        uint8_t satellites_used;         
 
        static constexpr uint8_t padding = 5; 
    };
 
    struct VehicleAttitude
    {
        uint64_t timestamp;                 
        std::array<float, 4> q;             
        std::array<float, 4> delta_q_reset; 
        uint8_t quat_reset_counter;         
 
        static constexpr uint8_t padding = 7; 
    };
 
    struct VehicleAirData
    {
        uint64_t timestamp;        
        uint64_t timestamp_sample; 
        uint32_t baro_device_id;   
        float baro_alt_meter;      
        float baro_temp_celcius;   
        float baro_pressure_pa;    
        float rho;                 
 
        static constexpr uint8_t padding = 4; 
    };
 
    struct VehicleControlMode
    {
        uint64_t timestamp;                         
        bool flag_armed;                            
        bool flag_external_manual_override_ok;      
        bool flag_control_manual_enabled;           
        bool flag_control_auto_enabled;             
        bool flag_control_offboard_enabled;         
        bool flag_control_rates_enabled;            
        bool flag_control_attitude_enabled;         
        bool flag_control_yawrate_override_enabled; 
        bool flag_control_rattitude_enabled;        
        bool flag_control_force_enabled;            
        bool flag_control_acceleration_enabled;     
        bool flag_control_velocity_enabled;         
        bool flag_control_position_enabled;         
        bool flag_control_altitude_enabled;         
        bool flag_control_climb_rate_enabled;       
        bool flag_control_termination_enabled;      
        bool flag_control_fixed_hdg_enabled;        
 
        static constexpr uint8_t padding = 7; 
    };
 
    struct VehicleStatus
    {
        uint64_t timestamp;                       
        uint64_t nav_state_timestamp;             
        uint32_t onboard_control_sensors_present; 
        uint32_t onboard_control_sensors_enabled; 
        uint32_t onboard_control_sensors_health;  
        uint8_t nav_state;                        
        uint8_t arming_state;                     
        uint8_t hil_state;                        
        bool failsafe;                            
        uint8_t system_type;                      
        uint8_t system_id;                        
        uint8_t component_id;                     
        uint8_t vehicle_type;                     
        bool is_vtol;                             
        bool is_vtol_tailsitter;                  
        bool vtol_fw_permanent_stab;              
        bool in_transition_mode;                  
        bool in_transition_to_fw;                 
        bool rc_signal_lost;                      
        uint8_t rc_input_mode;                    
        bool data_link_lost;                      
        uint8_t data_link_lost_counter;           
        bool high_latency_data_link_lost;         
        bool engine_failure;                      
        bool mission_failure;                     
        uint8_t failure_detector_status;          
        uint8_t latest_arming_reason;             
        uint8_t latest_disarming_reason;          
        std::array<uint8_t, 5> _padding0;         
    };
 
    struct Cpuload
    {
        uint64_t timestamp; 
        float load;         
        float ram_usage;    
    };
 
    std::unordered_map<uint16_t, SubscriptionData> _subscribedMessages;
 
    struct MeasurementData
    {
        uint8_t multi_id;                                                                           
        std::string message_name;                                                                   
        std::variant<SensorAccel, SensorGyro, SensorMag, VehicleGpsPosition, VehicleAttitude> data; 
    };
 
    std::multimap<uint64_t, MeasurementData> _epochData;
 
    int8_t enoughImuDataAvailable();
 
    std::array<std::multimap<uint64_t, NAV::UlogFile::MeasurementData>::iterator, 2> findPosVelAttData();
 
    struct
    {
        uint64_t timeSinceStartup{}; 
        InsTime gnssTime;            
    } lastGnssTime;
};
} // namespace NAV