| 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 KvhObs.hpp | ||
| 10 | /// @brief Data storage class for one KVH Imu observation | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2020-06-30 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "ImuObs.hpp" | ||
| 17 | #include "uart/protocol/packet.hpp" | ||
| 18 | #include <bitset> | ||
| 19 | |||
| 20 | namespace NAV | ||
| 21 | { | ||
| 22 | /// Kvh Observation storage Class | ||
| 23 | class KvhObs final : public ImuObs | ||
| 24 | { | ||
| 25 | public: | ||
| 26 | /// @brief Constructor | ||
| 27 | /// @param[in] imuPos Reference to the position and rotation info of the Imu | ||
| 28 | /// @param[in] packet The packet to copy into the raw data | ||
| 29 | ✗ | KvhObs(const ImuPos& imuPos, uart::protocol::Packet& packet) | |
| 30 | ✗ | : ImuObs(imuPos), raw(packet) {} | |
| 31 | |||
| 32 | /// @brief Constructor | ||
| 33 | /// @param[in] imuPos Reference to the position and rotation info of the Imu | ||
| 34 | ✗ | explicit KvhObs(const ImuPos& imuPos) | |
| 35 | ✗ | : ImuObs(imuPos) {} | |
| 36 | |||
| 37 | /// @brief Returns the type of the data class | ||
| 38 | /// @return The data type | ||
| 39 | 758 | [[nodiscard]] static std::string type() | |
| 40 | { | ||
| 41 |
1/2✓ Branch 1 taken 758 times.
✗ Branch 2 not taken.
|
1516 | return "KvhObs"; |
| 42 | } | ||
| 43 | |||
| 44 | /// @brief Returns the type of the data class | ||
| 45 | /// @return The data type | ||
| 46 | ✗ | [[nodiscard]] std::string getType() const override { return type(); } | |
| 47 | |||
| 48 | /// @brief Returns the parent types of the data class | ||
| 49 | /// @return The parent data types | ||
| 50 | 112 | [[nodiscard]] static std::vector<std::string> parentTypes() | |
| 51 | { | ||
| 52 |
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 { ImuObs::type() }; |
| 53 | 112 | } | |
| 54 | |||
| 55 | /// @brief Returns a vector of data descriptors | ||
| 56 | ✗ | [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors() | |
| 57 | { | ||
| 58 | ✗ | auto desc = ImuObs::GetStaticDataDescriptors(); | |
| 59 | ✗ | desc.emplace_back("Status [bits]"); | |
| 60 | ✗ | desc.emplace_back("Sequence Number [-]"); | |
| 61 | ✗ | return desc; | |
| 62 | ✗ | } | |
| 63 | |||
| 64 | /// @brief Get the amount of descriptors | ||
| 65 | ✗ | [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return ImuObs::GetStaticDescriptorCount() + 2; } | |
| 66 | |||
| 67 | /// @brief Returns a vector of data descriptors | ||
| 68 | ✗ | [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); } | |
| 69 | |||
| 70 | /// @brief Get the amount of descriptors | ||
| 71 | ✗ | [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); } | |
| 72 | |||
| 73 | /// @brief Get the value at the index | ||
| 74 | /// @param idx Index corresponding to data descriptor order | ||
| 75 | /// @return Value if in the observation | ||
| 76 | ✗ | [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override | |
| 77 | { | ||
| 78 | ✗ | INS_ASSERT(idx < GetStaticDescriptorCount()); | |
| 79 | ✗ | if (idx < ImuObs::GetStaticDescriptorCount()) { return ImuObs::getValueAt(idx); } | |
| 80 | ✗ | switch (idx) | |
| 81 | { | ||
| 82 | ✗ | case ImuObs::GetStaticDescriptorCount() + 0: // Status [bits] | |
| 83 | ✗ | return static_cast<double>(status.to_ulong()); | |
| 84 | ✗ | case ImuObs::GetStaticDescriptorCount() + 1: // Sequence Number [-] | |
| 85 | ✗ | if (sequenceNumber < 128) { return sequenceNumber; } | |
| 86 | ✗ | break; | |
| 87 | ✗ | default: | |
| 88 | ✗ | return std::nullopt; | |
| 89 | } | ||
| 90 | ✗ | return std::nullopt; | |
| 91 | } | ||
| 92 | |||
| 93 | /// Complete message raw binary data including header and checksum | ||
| 94 | uart::protocol::Packet raw; | ||
| 95 | |||
| 96 | /// @brief Status Byte | ||
| 97 | /// | ||
| 98 | /// Bit | Function | Notes | ||
| 99 | /// 0 | Gyro X status | 1 = Valid data, 0 = Invalid data | ||
| 100 | /// 1 | Gyro Y status | 1 = Valid data, 0 = Invalid data | ||
| 101 | /// 2 | Gyro Z status | 1 = Valid data, 0 = Invalid data | ||
| 102 | /// 3 | Reserved | Always 0 | ||
| 103 | /// 4 | Accelerometer X status | 1 = Valid data, 0 = Invalid data | ||
| 104 | /// 5 | Accelerometer Y status | 1 = Valid data, 0 = Invalid data | ||
| 105 | /// 6 | Accelerometer Z status | 1 = Valid data, 0 = Invalid data | ||
| 106 | /// 7 | Reserved | Always 0 | ||
| 107 | std::bitset<8> status; | ||
| 108 | |||
| 109 | /// Increments for each message and resets to 0 after 127 | ||
| 110 | uint8_t sequenceNumber = UINT8_MAX; | ||
| 111 | }; | ||
| 112 | |||
| 113 | } // namespace NAV | ||
| 114 |