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:
    using Scalar = long double; 
 
    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(Scalar time, const Eigen::Vector3<Scalar>& lla_position);
 
    // ###########################################################################################################
 
    enum class StartTimeSource
    {
        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
    {
        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
    {
        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
    {
        Duration,                 
        DistanceOrCirclesOrRoses, 
    };
 
    StopCondition _simulationStopCondition = StopCondition::Duration;
 
    double _simulationDuration = 5 * 60;
 
    Scalar _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::Vector3<Scalar> e_getPositionFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const;
 
    static Eigen::Quaternion<Scalar> n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine& line, const std::vector<std::string>& description);
 
    struct
    {
        Scalar sampleInterval = 0.03; 
        CubicSpline<Scalar> x;        
        CubicSpline<Scalar> y;        
        CubicSpline<Scalar> z;        
        CubicSpline<Scalar> roll;     
        CubicSpline<Scalar> pitch;    
        CubicSpline<Scalar> yaw;      
    } _splines;
 
    bool initializeSplines();
 
    uint64_t _imuInternalUpdateCnt = 0.0;
    uint64_t _imuUpdateCnt = 0.0;
    uint64_t _gnssUpdateCnt = 0.0;
 
    static constexpr Scalar INTERNAL_LINEAR_UPDATE_FREQUENCY = 1000;
 
    Scalar _imuLastUpdateTime = 0.0;
    Scalar _gnssLastUpdateTime = 0.0;
    Eigen::Vector3<Scalar> _lla_imuLastLinearPosition = Eigen::Vector3<Scalar>::Zero();
    Eigen::Vector3<Scalar> _lla_gnssLastLinearPosition = Eigen::Vector3<Scalar>::Zero();
    Eigen::Vector3<Scalar> _p_lastImuAccelerationMeas = Eigen::Vector3<Scalar>::Ones() * std::nan("");
    Eigen::Vector3<Scalar> _p_lastImuAngularRateMeas = Eigen::Vector3<Scalar>::Ones() * std::nan("");
 
    [[nodiscard]] std::array<Scalar, 3> calcFlightAngles(Scalar time) const;
 
    [[nodiscard]] Eigen::Vector3<Scalar> lla_calcPosition(Scalar time) const;
 
    [[nodiscard]] Eigen::Vector3<Scalar> n_calcVelocity(Scalar time, const Eigen::Quaternion<Scalar>& n_Quat_e) const;
 
    [[nodiscard]] Eigen::Vector3<Scalar> n_calcTrajectoryAccel(Scalar time, const Eigen::Quaternion<Scalar>& n_Quat_e,
                                                               const Eigen::Vector3<Scalar>& lla_position, const Eigen::Vector3<Scalar>& n_velocity) const;
 
    [[nodiscard]] Eigen::Vector3<Scalar> n_calcOmega_nb(Scalar time, const Eigen::Vector3<Scalar>& rollPitchYaw, const Eigen::Quaternion<Scalar>& n_Quat_b) const;
};
 
} // namespace NAV