ImuSimulator.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 "Nodes/DataProvider/IMU/Imu.hpp"
 
#include "util/Eigen.hpp"
#include "Navigation/Time/InsTime.hpp"
#include "Navigation/Gravity/Gravity.hpp"
#include "Navigation/Math/CubicSpline.hpp"
#include "internal/gui/widgets/TimeEdit.hpp"
#include "internal/gui/widgets/PositionInput.hpp"
 
#include "NodeData/General/CsvData.hpp"
 
#include <array>
 
namespace NAV
{
class ImuSimulator : public Imu
{
  public:
    ImuSimulator();
    ~ImuSimulator() override;
    ImuSimulator(const ImuSimulator&) = delete;
    ImuSimulator(ImuSimulator&&) = delete;
    ImuSimulator& operator=(const ImuSimulator&) = delete;
    ImuSimulator& operator=(ImuSimulator&&) = 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;
 
  private:
    constexpr static size_t INPUT_PORT_INDEX_CSV = 0;          
    constexpr static size_t OUTPUT_PORT_INDEX_IMU_OBS = 0;     
    constexpr static size_t OUTPUT_PORT_INDEX_POS_VEL_ATT = 1; 
 
    bool initialize() override;
 
    void deinitialize() override;
 
    [[nodiscard]] std::shared_ptr<const NodeData> pollImuObs(size_t pinIdx, bool peek);
 
    [[nodiscard]] std::shared_ptr<const NodeData> pollPosVelAtt(size_t pinIdx, bool peek);
 
    bool checkStopCondition(double time, const Eigen::Vector3d& lla_position);
 
    // ###########################################################################################################
 
    enum class StartTimeSource : uint8_t
    {
        CustomTime,          
        CurrentComputerTime, 
    };
 
    StartTimeSource _startTimeSource = StartTimeSource::CustomTime;
 
    gui::widgets::TimeEditFormat _startTimeEditFormat;
 
    InsTime _startTime{ 2000, 1, 1, 0, 0, 0 };
 
    // ###########################################################################################################
 
    double _imuInternalFrequency = 1000;
    double _imuFrequency = 100;
    double _gnssFrequency = 5;
 
    // ###########################################################################################################
 
    enum class TrajectoryType : uint8_t
    {
        Fixed,      
        Linear,     
        Circular,   
        Csv,        
        RoseFigure, 
        COUNT,      
    };
    static const char* to_string(TrajectoryType value);
 
    TrajectoryType _trajectoryType = TrajectoryType::Fixed;
 
    gui::widgets::PositionWithFrame _startPosition;
 
    Eigen::Vector3d _fixedTrajectoryStartOrientation = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d _n_linearTrajectoryStartVelocity = Eigen::Vector3d{ 1, 0, 0 };
 
    int _circularHarmonicFrequency = 0;
 
    double _circularHarmonicAmplitudeFactor = 0.1;
 
    enum class Direction : uint8_t
    {
        CW,    
        CCW,   
        COUNT, 
    };
    static const char* to_string(Direction value);
 
    Direction _trajectoryDirection = Direction::CCW;
 
    double _trajectoryRotationAngle = 0.0;
 
    double _trajectoryHorizontalSpeed = 10.0;
 
    double _trajectoryVerticalSpeed = 0.0;
 
    double _trajectoryRadius = 50.0;
 
    int _rosePetNum = 2;
 
    int _rosePetDenom = 1;
 
    double _roseStepLengthMax = 0.1;
 
    double _roseSimDuration = 0.0;
 
    // ###########################################################################################################
 
    enum StopCondition : uint8_t
    {
        Duration,                 
        DistanceOrCirclesOrRoses, 
    };
 
    StopCondition _simulationStopCondition = StopCondition::Duration;
 
    double _simulationDuration = 5 * 60;
 
    double _csvDuration = 0;
 
    double _linearTrajectoryDistanceForStop = 100;
 
    double _circularTrajectoryCircleCountForStop = 1.0;
 
    double _roseTrajectoryCountForStop = 1.0;
    // ###########################################################################################################
 
    GravitationModel _gravitationModel = GravitationModel::EGM96;
 
    bool _coriolisAccelerationEnabled = true;
 
    bool _centrifgalAccelerationEnabled = true;
 
    bool _angularRateEarthRotationEnabled = true;
 
    bool _angularRateTransportRateEnabled = true;
 
    // ###########################################################################################################
 
    [[nodiscard]] InsTime getTimeFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const;
 
    [[nodiscard]] Eigen::Vector3d e_getPositionFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const;
 
    static Eigen::Quaterniond n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine& line, const std::vector<std::string>& description);
 
    struct
    {
        double sampleInterval = 0.03;   
        CubicSpline<long double> x;     
        CubicSpline<long double> y;     
        CubicSpline<long double> z;     
        CubicSpline<long double> roll;  
        CubicSpline<long double> pitch; 
        CubicSpline<long double> yaw;   
    } _splines;
 
    bool initializeSplines();
 
    uint64_t _imuInternalUpdateCnt = 0.0;
    uint64_t _imuUpdateCnt = 0.0;
    uint64_t _gnssUpdateCnt = 0.0;
 
    static constexpr double INTERNAL_LINEAR_UPDATE_FREQUENCY = 1000;
 
    double _imuLastUpdateTime = 0.0;
    double _gnssLastUpdateTime = 0.0;
    Eigen::Vector3d _lla_imuLastLinearPosition = Eigen::Vector3d::Zero();
    Eigen::Vector3d _lla_gnssLastLinearPosition = Eigen::Vector3d::Zero();
    Eigen::Vector3d _p_lastImuAccelerationMeas = Eigen::Vector3d::Ones() * std::nan("");
    Eigen::Vector3d _p_lastImuAngularRateMeas = Eigen::Vector3d::Ones() * std::nan("");
 
    [[nodiscard]] std::array<double, 3> calcFlightAngles(double time) const;
 
    [[nodiscard]] Eigen::Vector3d lla_calcPosition(double time) const;
 
    [[nodiscard]] Eigen::Vector3d n_calcVelocity(double time, const Eigen::Quaterniond& n_Quat_e) const;
 
    [[nodiscard]] Eigen::Vector3d n_calcTrajectoryAccel(double time, const Eigen::Quaterniond& n_Quat_e,
                                                        const Eigen::Vector3d& lla_position, const Eigen::Vector3d& n_velocity) const;
 
    [[nodiscard]] Eigen::Vector3d n_calcOmega_nb(double time, const Eigen::Vector3d& rollPitchYaw, const Eigen::Quaterniond& n_Quat_b) const;
};
 
} // namespace NAV