| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | /// @file ImuSimulator.hpp | ||
| 10 | /// @brief Imu Observation Simulator | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @author N. Stahl (Hiwi: Rose figure trajectory type) | ||
| 13 | /// @date 2023-07-18 | ||
| 14 | |||
| 15 | #pragma once | ||
| 16 | |||
| 17 | #include "Nodes/DataProvider/IMU/Imu.hpp" | ||
| 18 | |||
| 19 | #include "util/Eigen.hpp" | ||
| 20 | #include "Navigation/Time/InsTime.hpp" | ||
| 21 | #include "Navigation/Gravity/Gravity.hpp" | ||
| 22 | #include "Navigation/Math/CubicSpline.hpp" | ||
| 23 | #include "internal/gui/widgets/TimeEdit.hpp" | ||
| 24 | #include "internal/gui/widgets/PositionInput.hpp" | ||
| 25 | |||
| 26 | #include "NodeData/General/CsvData.hpp" | ||
| 27 | |||
| 28 | #include <array> | ||
| 29 | |||
| 30 | namespace NAV | ||
| 31 | { | ||
| 32 | /// Imu Observation Simulator | ||
| 33 | class ImuSimulator : public Imu | ||
| 34 | { | ||
| 35 | public: | ||
| 36 | /// @brief Default constructor | ||
| 37 | ImuSimulator(); | ||
| 38 | /// @brief Destructor | ||
| 39 | ~ImuSimulator() override; | ||
| 40 | /// @brief Copy constructor | ||
| 41 | ImuSimulator(const ImuSimulator&) = delete; | ||
| 42 | /// @brief Move constructor | ||
| 43 | ImuSimulator(ImuSimulator&&) = delete; | ||
| 44 | /// @brief Copy assignment operator | ||
| 45 | ImuSimulator& operator=(const ImuSimulator&) = delete; | ||
| 46 | /// @brief Move assignment operator | ||
| 47 | ImuSimulator& operator=(ImuSimulator&&) = delete; | ||
| 48 | |||
| 49 | /// @brief String representation of the Class Type | ||
| 50 | [[nodiscard]] static std::string typeStatic(); | ||
| 51 | |||
| 52 | /// @brief String representation of the Class Type | ||
| 53 | [[nodiscard]] std::string type() const override; | ||
| 54 | |||
| 55 | /// @brief String representation of the Class Category | ||
| 56 | [[nodiscard]] static std::string category(); | ||
| 57 | |||
| 58 | /// @brief ImGui config window which is shown on double click | ||
| 59 | /// @attention Don't forget to set _hasConfig to true in the constructor of the node | ||
| 60 | void guiConfig() override; | ||
| 61 | |||
| 62 | /// @brief Saves the node into a json object | ||
| 63 | [[nodiscard]] json save() const override; | ||
| 64 | |||
| 65 | /// @brief Restores the node from a json object | ||
| 66 | /// @param[in] j Json object with the node state | ||
| 67 | void restore(const json& j) override; | ||
| 68 | |||
| 69 | /// @brief Resets the node. Moves the read cursor to the start | ||
| 70 | bool resetNode() override; | ||
| 71 | |||
| 72 | private: | ||
| 73 | constexpr static size_t INPUT_PORT_INDEX_CSV = 0; ///< @brief Object (CsvData) | ||
| 74 | constexpr static size_t OUTPUT_PORT_INDEX_IMU_OBS = 0; ///< @brief Flow (ImuObs) | ||
| 75 | constexpr static size_t OUTPUT_PORT_INDEX_POS_VEL_ATT = 1; ///< @brief Flow (PosVelAtt) | ||
| 76 | |||
| 77 | /// @brief Initialize the node | ||
| 78 | bool initialize() override; | ||
| 79 | |||
| 80 | /// @brief Deinitialize the node | ||
| 81 | void deinitialize() override; | ||
| 82 | |||
| 83 | /// @brief Polls the next simulated data | ||
| 84 | /// @param[in] pinIdx Index of the pin the data is requested on | ||
| 85 | /// @param[in] peek Specifies if the data should be peeked or read | ||
| 86 | /// @return The simulated observation | ||
| 87 | [[nodiscard]] std::shared_ptr<const NodeData> pollImuObs(size_t pinIdx, bool peek); | ||
| 88 | |||
| 89 | /// @brief Polls the next simulated data | ||
| 90 | /// @param[in] pinIdx Index of the pin the data is requested on | ||
| 91 | /// @param[in] peek Specifies if the data should be peeked or read | ||
| 92 | /// @return The simulated observation | ||
| 93 | [[nodiscard]] std::shared_ptr<const NodeData> pollPosVelAtt(size_t pinIdx, bool peek); | ||
| 94 | |||
| 95 | /// @brief Checks the selected stop condition | ||
| 96 | /// @param[in] time Current simulation time | ||
| 97 | /// @param[in] lla_position Current position | ||
| 98 | /// @return True if it should be stopped | ||
| 99 | bool checkStopCondition(double time, const Eigen::Vector3d& lla_position); | ||
| 100 | |||
| 101 | // ########################################################################################################### | ||
| 102 | |||
| 103 | /// Types where the start time should be pulled from | ||
| 104 | enum class StartTimeSource : uint8_t | ||
| 105 | { | ||
| 106 | CustomTime, ///< Custom time selected by the user | ||
| 107 | CurrentComputerTime, ///< Gets the current computer time as start time | ||
| 108 | }; | ||
| 109 | |||
| 110 | /// Source for the start time, selected in the GUI | ||
| 111 | StartTimeSource _startTimeSource = StartTimeSource::CustomTime; | ||
| 112 | |||
| 113 | /// Time Format to input the start time with | ||
| 114 | gui::widgets::TimeEditFormat _startTimeEditFormat; | ||
| 115 | |||
| 116 | /// Global starttime | ||
| 117 | InsTime _startTime{ 2000, 1, 1, 0, 0, 0 }; | ||
| 118 | |||
| 119 | // ########################################################################################################### | ||
| 120 | |||
| 121 | /// Frequency to calculate the delta IMU values in [Hz] | ||
| 122 | double _imuInternalFrequency = 1000; | ||
| 123 | /// Frequency to sample the IMU with in [Hz] | ||
| 124 | double _imuFrequency = 100; | ||
| 125 | /// Frequency to sample the position with in [Hz] | ||
| 126 | double _gnssFrequency = 5; | ||
| 127 | |||
| 128 | // ########################################################################################################### | ||
| 129 | |||
| 130 | /// Types of Trajectories available for simulation | ||
| 131 | enum class TrajectoryType : uint8_t | ||
| 132 | { | ||
| 133 | Fixed, ///< Static position without movement | ||
| 134 | Linear, ///< Linear movement with constant velocity | ||
| 135 | Circular, ///< Circular path | ||
| 136 | Csv, ///< Get the input from the CsvData pin | ||
| 137 | RoseFigure, ///< Movement along a mathmatical rose figure | ||
| 138 | COUNT, ///< Amount of items in the enum | ||
| 139 | }; | ||
| 140 | /// @brief Converts the enum to a string | ||
| 141 | /// @param[in] value Enum value to convert into text | ||
| 142 | /// @return String representation of the enum | ||
| 143 | static const char* to_string(TrajectoryType value); | ||
| 144 | |||
| 145 | /// Selected trajectory type in the GUI | ||
| 146 | TrajectoryType _trajectoryType = TrajectoryType::Fixed; | ||
| 147 | |||
| 148 | /// Start position in local navigation coordinates (latitude, longitude, altitude) [rad, rad, m] | ||
| 149 | /// | ||
| 150 | /// - Fixed, Linear: Start position | ||
| 151 | /// - Circular: Center of the circle | ||
| 152 | gui::widgets::PositionWithFrame _startPosition; | ||
| 153 | |||
| 154 | /// Orientation of the vehicle [roll, pitch, yaw] [rad] | ||
| 155 | Eigen::Vector3d _fixedTrajectoryStartOrientation = Eigen::Vector3d::Zero(); | ||
| 156 | |||
| 157 | /// Start Velocity of the vehicle in local-navigation frame cooridnates in [m/s] | ||
| 158 | Eigen::Vector3d _n_linearTrajectoryStartVelocity = Eigen::Vector3d{ 1, 0, 0 }; | ||
| 159 | |||
| 160 | /// Harmonic Oscillation Frequency on the circular trajectory [cycles/revolution] | ||
| 161 | int _circularHarmonicFrequency = 0; | ||
| 162 | |||
| 163 | /// Harmonic Oscillation Amplitude Factor of the circle radius [-] | ||
| 164 | double _circularHarmonicAmplitudeFactor = 0.1; | ||
| 165 | |||
| 166 | /// Possible directions for the circular trajectory | ||
| 167 | enum class Direction : uint8_t | ||
| 168 | { | ||
| 169 | CW, ///< Clockwise | ||
| 170 | CCW, ///< Counterclockwise | ||
| 171 | COUNT, ///< Amount of items in the enum | ||
| 172 | }; | ||
| 173 | /// @brief Converts the enum to a string | ||
| 174 | /// @param[in] value Enum value to convert into text | ||
| 175 | /// @return String representation of the enum | ||
| 176 | static const char* to_string(Direction value); | ||
| 177 | |||
| 178 | /// In the GUI selected direction of the circular trajectory (used by circular and rose figure) | ||
| 179 | Direction _trajectoryDirection = Direction::CCW; | ||
| 180 | |||
| 181 | /// In the GUI selected origin angle of the circular trajectory in [rad] | ||
| 182 | double _trajectoryRotationAngle = 0.0; | ||
| 183 | |||
| 184 | /// Horizontal speed of the vehicle in the tangential plane in [m/s] (used by circular and rose figure) | ||
| 185 | double _trajectoryHorizontalSpeed = 10.0; | ||
| 186 | |||
| 187 | /// Vertical speed of the vehicle in the tangential plane in [m/s] (used by circular and rose figure) | ||
| 188 | double _trajectoryVerticalSpeed = 0.0; | ||
| 189 | |||
| 190 | /// In the GUI selected radius of the circular trajectory (used by circular and rose figure) | ||
| 191 | double _trajectoryRadius = 50.0; | ||
| 192 | |||
| 193 | /// In the GUI selected numerator of petals (2*k for even k, k for uneven k) of the rose figure | ||
| 194 | int _rosePetNum = 2; | ||
| 195 | |||
| 196 | /// In the GUI selected denominator of petals (2*k for even k, k for uneven k) of the rose figure | ||
| 197 | int _rosePetDenom = 1; | ||
| 198 | |||
| 199 | /// Maxmimum step length for the spline points for the rose figure [m]. Points will be spaced between [L/3 L] | ||
| 200 | double _roseStepLengthMax = 0.1; | ||
| 201 | |||
| 202 | /// Simulation duration needed for the rose figure | ||
| 203 | double _roseSimDuration = 0.0; | ||
| 204 | |||
| 205 | // ########################################################################################################### | ||
| 206 | |||
| 207 | /// Possible stop conditions for the simulation | ||
| 208 | enum StopCondition : uint8_t | ||
| 209 | { | ||
| 210 | Duration, ///< Time Duration | ||
| 211 | DistanceOrCirclesOrRoses, ///< Distance for Linear trajectory / Circle count for Circular / Count for rose figure trajectory | ||
| 212 | }; | ||
| 213 | |||
| 214 | /// Condition which has to be met to stop the simulation | ||
| 215 | StopCondition _simulationStopCondition = StopCondition::Duration; | ||
| 216 | |||
| 217 | /// Duration to simulate in [s] | ||
| 218 | double _simulationDuration = 5 * 60; | ||
| 219 | |||
| 220 | /// Duration from the CSV file in [s] | ||
| 221 | double _csvDuration = 0; | ||
| 222 | |||
| 223 | /// Distance in [m] to the start position to stop the simulation | ||
| 224 | double _linearTrajectoryDistanceForStop = 100; | ||
| 225 | |||
| 226 | /// Amount of circles to simulate before stopping | ||
| 227 | double _circularTrajectoryCircleCountForStop = 1.0; | ||
| 228 | |||
| 229 | /// Amount of rose figures to simulate before stopping | ||
| 230 | double _roseTrajectoryCountForStop = 1.0; | ||
| 231 | // ########################################################################################################### | ||
| 232 | |||
| 233 | /// Gravitation model selected in the GUI | ||
| 234 | GravitationModel _gravitationModel = GravitationModel::EGM96; | ||
| 235 | |||
| 236 | /// Apply the coriolis acceleration to the measured accelerations | ||
| 237 | bool _coriolisAccelerationEnabled = true; | ||
| 238 | |||
| 239 | /// Apply the centrifugal acceleration to the measured accelerations | ||
| 240 | bool _centrifgalAccelerationEnabled = true; | ||
| 241 | |||
| 242 | /// Apply the Earth rotation rate to the measured angular rates | ||
| 243 | bool _angularRateEarthRotationEnabled = true; | ||
| 244 | |||
| 245 | /// Apply the transport rate to the measured angular rates | ||
| 246 | bool _angularRateTransportRateEnabled = true; | ||
| 247 | |||
| 248 | // ########################################################################################################### | ||
| 249 | |||
| 250 | /// @brief Get the Time from a CSV line | ||
| 251 | /// @param[in] line Line with data from the csv | ||
| 252 | /// @param[in] description Description of the data | ||
| 253 | /// @return InsTime or empty time if data not found | ||
| 254 | [[nodiscard]] InsTime getTimeFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const; | ||
| 255 | |||
| 256 | /// @brief Get the Position from a CSV line | ||
| 257 | /// @param[in] line Line with data from the csv | ||
| 258 | /// @param[in] description Description of the data | ||
| 259 | /// @return Position in ECEF coordinates in [m] or NaN if data not found | ||
| 260 | [[nodiscard]] Eigen::Vector3d e_getPositionFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const; | ||
| 261 | |||
| 262 | /// @brief Get the Attitude quaternion n_quat_b from a CSV line | ||
| 263 | /// @param[in] line Line with data from the csv | ||
| 264 | /// @param[in] description Description of the data | ||
| 265 | /// @return Attitude quaternion n_quat_b or NaN if data not found | ||
| 266 | static Eigen::Quaterniond n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine& line, const std::vector<std::string>& description); | ||
| 267 | |||
| 268 | /// Assign a variable that holds the Spline information | ||
| 269 | struct | ||
| 270 | { | ||
| 271 | double sampleInterval = 0.03; ///< Spline sample interval | ||
| 272 | CubicSpline<long double> x; ///< ECEF X Position [m] | ||
| 273 | CubicSpline<long double> y; ///< ECEF Y Position [m] | ||
| 274 | CubicSpline<long double> z; ///< ECEF Z Position [m] | ||
| 275 | CubicSpline<long double> roll; ///< Roll angle [rad] | ||
| 276 | CubicSpline<long double> pitch; ///< Pitch angle [rad] | ||
| 277 | CubicSpline<long double> yaw; ///< Yaw angle [rad] | ||
| 278 | } _splines; | ||
| 279 | |||
| 280 | /// @brief Initializes the spline values | ||
| 281 | /// @return True if everything succeeded | ||
| 282 | bool initializeSplines(); | ||
| 283 | |||
| 284 | /// Counter to calculate the internal IMU update time | ||
| 285 | uint64_t _imuInternalUpdateCnt = 0.0; | ||
| 286 | /// Counter to calculate the IMU update time | ||
| 287 | uint64_t _imuUpdateCnt = 0.0; | ||
| 288 | /// Counter to calculate the GNSS update time | ||
| 289 | uint64_t _gnssUpdateCnt = 0.0; | ||
| 290 | |||
| 291 | /// Update rate for the internal solution of linear movement in [Hz] | ||
| 292 | static constexpr double INTERNAL_LINEAR_UPDATE_FREQUENCY = 1000; | ||
| 293 | |||
| 294 | /// Last time the IMU message was calculated in [s] | ||
| 295 | double _imuLastUpdateTime = 0.0; | ||
| 296 | /// Last time the GNSS message was calculated in [s] | ||
| 297 | double _gnssLastUpdateTime = 0.0; | ||
| 298 | /// Last calculated position for the IMU in linear mode for iterative calculations as latitude, longitude, altitude [rad, rad, m] | ||
| 299 | Eigen::Vector3d _lla_imuLastLinearPosition = Eigen::Vector3d::Zero(); | ||
| 300 | /// Last calculated position for the GNSS in linear mode for iterative calculations as latitude, longitude, altitude [rad, rad, m] | ||
| 301 | Eigen::Vector3d _lla_gnssLastLinearPosition = Eigen::Vector3d::Zero(); | ||
| 302 | /// Last calculated acceleration measurement in platform coordinates [m/s²] | ||
| 303 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | Eigen::Vector3d _p_lastImuAccelerationMeas = Eigen::Vector3d::Ones() * std::nan(""); |
| 304 | /// Last calculated angular rate measurement in platform coordinates [rad/s] | ||
| 305 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | Eigen::Vector3d _p_lastImuAngularRateMeas = Eigen::Vector3d::Ones() * std::nan(""); |
| 306 | |||
| 307 | /// @brief Calculates the flight angles (roll, pitch, yaw) | ||
| 308 | /// @param[in] time Time in [s] | ||
| 309 | /// @return Roll, pitch, yaw in [rad] | ||
| 310 | [[nodiscard]] std::array<double, 3> calcFlightAngles(double time) const; | ||
| 311 | |||
| 312 | /// @brief Calculates the position in latLonAlt at the given time depending on the trajectoryType | ||
| 313 | /// @param[in] time Time in [s] | ||
| 314 | /// @return LatLonAlt in [rad, rad, m] | ||
| 315 | [[nodiscard]] Eigen::Vector3d lla_calcPosition(double time) const; | ||
| 316 | |||
| 317 | /// @brief Calculates the velocity in local-navigation frame coordinates at the given time depending on the trajectoryType | ||
| 318 | /// @param[in] time Time in [s] | ||
| 319 | /// @param[in] n_Quat_e Rotation quaternion from Earth frame to local-navigation frame | ||
| 320 | /// @return n_velocity in [rad, rad, m] | ||
| 321 | [[nodiscard]] Eigen::Vector3d n_calcVelocity(double time, const Eigen::Quaterniond& n_Quat_e) const; | ||
| 322 | |||
| 323 | /// @brief Calculates the acceleration in local-navigation frame coordinates at the given time depending on the trajectoryType | ||
| 324 | /// @param[in] time Time in [s] | ||
| 325 | /// @param[in] n_Quat_e Rotation quaternion from Earth frame to local-navigation frame | ||
| 326 | /// @param[in] lla_position Current position as latitude, longitude, altitude [rad, rad, m] | ||
| 327 | /// @param[in] n_velocity Velocity in local-navigation frame coordinates [m/s] | ||
| 328 | /// @return n_accel in [rad, rad, m] | ||
| 329 | [[nodiscard]] Eigen::Vector3d n_calcTrajectoryAccel(double time, const Eigen::Quaterniond& n_Quat_e, | ||
| 330 | const Eigen::Vector3d& lla_position, const Eigen::Vector3d& n_velocity) const; | ||
| 331 | |||
| 332 | /// @brief Calculates ω_nb_n, the turn rate of the body with respect to the navigation system expressed in NED coordinates | ||
| 333 | /// @param[in] time Time in [s] | ||
| 334 | /// @param[in] rollPitchYaw Gimbal angles (roll, pitch, yaw) [rad] | ||
| 335 | /// @param[in] n_Quat_b Rotation quaternion from body frame to the local-navigation frame | ||
| 336 | /// @return ω_nb_n [rad/s] | ||
| 337 | [[nodiscard]] Eigen::Vector3d n_calcOmega_nb(double time, const Eigen::Vector3d& rollPitchYaw, const Eigen::Quaterniond& n_Quat_b) const; | ||
| 338 | }; | ||
| 339 | |||
| 340 | } // namespace NAV | ||
| 341 |