| 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 ImuObs.hpp | ||
| 10 | /// @brief Parent Class for all IMU Observations | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2020-03-12 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "NodeData/NodeData.hpp" | ||
| 17 | |||
| 18 | #include "ImuPos.hpp" | ||
| 19 | #include "util/Eigen.hpp" | ||
| 20 | |||
| 21 | namespace NAV | ||
| 22 | { | ||
| 23 | /// IMU Observation storage class | ||
| 24 | class ImuObs : public NodeData | ||
| 25 | { | ||
| 26 | public: | ||
| 27 | /// @brief Constructor | ||
| 28 | /// @param[in] imuPos Reference to the position and rotation info of the Imu | ||
| 29 | 85956 | explicit ImuObs(const ImuPos& imuPos) | |
| 30 |
2/4✓ Branch 3 taken 85950 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 85955 times.
✗ Branch 7 not taken.
|
171902 | : imuPos(imuPos) {} |
| 31 | |||
| 32 | /// @brief Returns the type of the data class | ||
| 33 | /// @return The data type | ||
| 34 | 28093 | [[nodiscard]] static std::string type() | |
| 35 | { | ||
| 36 |
1/2✓ Branch 1 taken 28093 times.
✗ Branch 2 not taken.
|
56186 | return "ImuObs"; |
| 37 | } | ||
| 38 | |||
| 39 | /// @brief Returns the type of the data class | ||
| 40 | /// @return The data type | ||
| 41 | ✗ | [[nodiscard]] std::string getType() const override { return type(); } | |
| 42 | |||
| 43 | /// @brief Returns the parent types of the data class | ||
| 44 | /// @return The parent data types | ||
| 45 | 112 | [[nodiscard]] static std::vector<std::string> parentTypes() | |
| 46 | { | ||
| 47 |
3/6✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 112 times.
✓ Branch 4 taken 112 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
336 | return { NodeData::type() }; |
| 48 | 112 | } | |
| 49 | |||
| 50 | /// @brief Returns a vector of data descriptors | ||
| 51 | 12 | [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors() | |
| 52 | { | ||
| 53 | return { | ||
| 54 | "Time since startup [ns]", | ||
| 55 | "Accel X [m/s^2]", | ||
| 56 | "Accel Y [m/s^2]", | ||
| 57 | "Accel Z [m/s^2]", | ||
| 58 | "Gyro X [rad/s]", | ||
| 59 | "Gyro Y [rad/s]", | ||
| 60 | "Gyro Z [rad/s]", | ||
| 61 | "Mag X [Gauss]", | ||
| 62 | "Mag Y [Gauss]", | ||
| 63 | "Mag Z [Gauss]", | ||
| 64 | "Temperature [°C]", | ||
| 65 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
36 | }; |
| 66 | } | ||
| 67 | |||
| 68 | /// @brief Get the amount of descriptors | ||
| 69 | ✗ | [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 11; } | |
| 70 | |||
| 71 | /// @brief Returns a vector of data descriptors | ||
| 72 | ✗ | [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); } | |
| 73 | |||
| 74 | /// @brief Get the amount of descriptors | ||
| 75 | ✗ | [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); } | |
| 76 | |||
| 77 | /// @brief Get the value at the index | ||
| 78 | /// @param idx Index corresponding to data descriptor order | ||
| 79 | /// @return Value if in the observation | ||
| 80 | ✗ | [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override | |
| 81 | { | ||
| 82 | ✗ | INS_ASSERT(idx < GetStaticDescriptorCount()); | |
| 83 | ✗ | switch (idx) | |
| 84 | { | ||
| 85 | ✗ | case 0: // Time since startup [ns] | |
| 86 | ✗ | if (timeSinceStartup.has_value()) { return static_cast<double>(timeSinceStartup.value()); } | |
| 87 | ✗ | break; | |
| 88 | ✗ | case 1: // Accel X [m/s^2] | |
| 89 | ✗ | return p_acceleration.x(); | |
| 90 | ✗ | case 2: // Accel Y [m/s^2] | |
| 91 | ✗ | return p_acceleration.y(); | |
| 92 | ✗ | case 3: // Accel Z [m/s^2] | |
| 93 | ✗ | return p_acceleration.z(); | |
| 94 | ✗ | case 4: // Gyro X [rad/s] | |
| 95 | ✗ | return p_angularRate.x(); | |
| 96 | ✗ | case 5: // Gyro Y [rad/s] | |
| 97 | ✗ | return p_angularRate.y(); | |
| 98 | ✗ | case 6: // Gyro Z [rad/s] | |
| 99 | ✗ | return p_angularRate.z(); | |
| 100 | ✗ | case 7: // Mag X [Gauss] | |
| 101 | ✗ | if (p_magneticField.has_value()) { return p_magneticField->x(); } | |
| 102 | ✗ | break; | |
| 103 | ✗ | case 8: // Mag Y [Gauss] | |
| 104 | ✗ | if (p_magneticField.has_value()) { return p_magneticField->y(); } | |
| 105 | ✗ | break; | |
| 106 | ✗ | case 9: // Mag Z [Gauss] | |
| 107 | ✗ | if (p_magneticField.has_value()) { return p_magneticField->z(); } | |
| 108 | ✗ | break; | |
| 109 | ✗ | case 10: // Temperature [°C] | |
| 110 | ✗ | return temperature; | |
| 111 | ✗ | default: | |
| 112 | ✗ | return std::nullopt; | |
| 113 | } | ||
| 114 | ✗ | return std::nullopt; | |
| 115 | } | ||
| 116 | |||
| 117 | /// @brief Set the value at the index | ||
| 118 | /// @param idx Index corresponding to data descriptor order | ||
| 119 | /// @param value Value to set | ||
| 120 | /// @return True if the value was updated | ||
| 121 | ✗ | [[nodiscard]] bool setValueAt(size_t idx, double value) override | |
| 122 | { | ||
| 123 | ✗ | INS_ASSERT(idx < GetStaticDescriptorCount()); | |
| 124 | |||
| 125 | ✗ | switch (idx) | |
| 126 | { | ||
| 127 | ✗ | case 0: // Time since startup [ns] | |
| 128 | ✗ | if (value >= 0.0) | |
| 129 | { | ||
| 130 | ✗ | timeSinceStartup = static_cast<size_t>(value); | |
| 131 | ✗ | return true; | |
| 132 | } | ||
| 133 | ✗ | break; | |
| 134 | ✗ | case 1: // Accel X [m/s^2] | |
| 135 | ✗ | p_acceleration.x() = value; | |
| 136 | ✗ | return true; | |
| 137 | ✗ | case 2: // Accel Y [m/s^2] | |
| 138 | ✗ | p_acceleration.y() = value; | |
| 139 | ✗ | return true; | |
| 140 | ✗ | case 3: // Accel Z [m/s^2] | |
| 141 | ✗ | p_acceleration.z() = value; | |
| 142 | ✗ | return true; | |
| 143 | ✗ | case 4: // Gyro X [rad/s] | |
| 144 | ✗ | p_angularRate.x() = value; | |
| 145 | ✗ | return true; | |
| 146 | ✗ | case 5: // Gyro Y [rad/s] | |
| 147 | ✗ | p_angularRate.y() = value; | |
| 148 | ✗ | return true; | |
| 149 | ✗ | case 6: // Gyro Z [rad/s] | |
| 150 | ✗ | p_angularRate.z() = value; | |
| 151 | ✗ | return true; | |
| 152 | ✗ | case 7: // Mag X [Gauss] | |
| 153 | ✗ | if (p_magneticField.has_value()) | |
| 154 | { | ||
| 155 | ✗ | p_magneticField->x() = value; | |
| 156 | ✗ | return true; | |
| 157 | } | ||
| 158 | ✗ | break; | |
| 159 | ✗ | case 8: // Mag Y [Gauss] | |
| 160 | ✗ | if (p_magneticField.has_value()) | |
| 161 | { | ||
| 162 | ✗ | p_magneticField->y() = value; | |
| 163 | ✗ | return true; | |
| 164 | } | ||
| 165 | ✗ | break; | |
| 166 | ✗ | case 9: // Mag Z [Gauss] | |
| 167 | ✗ | if (p_magneticField.has_value()) | |
| 168 | { | ||
| 169 | ✗ | p_magneticField->z() = value; | |
| 170 | ✗ | return true; | |
| 171 | } | ||
| 172 | ✗ | break; | |
| 173 | ✗ | case 10: // Temperature [°C] | |
| 174 | ✗ | temperature = value; | |
| 175 | ✗ | return true; | |
| 176 | ✗ | default: | |
| 177 | ✗ | return false; | |
| 178 | } | ||
| 179 | |||
| 180 | ✗ | return false; | |
| 181 | } | ||
| 182 | |||
| 183 | /// Position and rotation information for conversion from platform to body frame | ||
| 184 | const ImuPos& imuPos; | ||
| 185 | |||
| 186 | /// The system time since startup measured in [nano seconds]. | ||
| 187 | std::optional<uint64_t> timeSinceStartup; | ||
| 188 | |||
| 189 | /// The IMU acceleration measured in units of [m/s^2], given in the platform frame. | ||
| 190 | Eigen::Vector3d p_acceleration; | ||
| 191 | /// The IMU angular rate measured in units of [rad/s], given in the platform frame. | ||
| 192 | Eigen::Vector3d p_angularRate; | ||
| 193 | |||
| 194 | /// The IMU magnetic field measured in units of [Gauss], given in the platform frame. | ||
| 195 | std::optional<Eigen::Vector3d> p_magneticField; | ||
| 196 | /// The IMU temperature measured in units of [Celsius]. | ||
| 197 | 85953 | std::optional<double> temperature = 0.0; | |
| 198 | }; | ||
| 199 | |||
| 200 | } // namespace NAV | ||
| 201 |