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