| 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 | #include "Navio2Sensor.hpp" | ||
| 10 | |||
| 11 | #include "util/Logger.hpp" | ||
| 12 | |||
| 13 | #if !__APPLE__ && !defined(WIN32) && !defined(_WIN32) && !defined(__WIN32) | ||
| 14 | #include "Navio/Common/MPU9250.h" | ||
| 15 | #include "Navio/Navio2/LSM9DS1.h" | ||
| 16 | #include "Navio/Common/Util.h" | ||
| 17 | #endif | ||
| 18 | |||
| 19 | #include "internal/NodeManager.hpp" | ||
| 20 | namespace nm = NAV::NodeManager; | ||
| 21 | #include "internal/FlowManager.hpp" | ||
| 22 | |||
| 23 | #include "NodeData/IMU/ImuObs.hpp" | ||
| 24 | |||
| 25 | #include "util/Time/TimeBase.hpp" | ||
| 26 | |||
| 27 | 112 | NAV::Navio2Sensor::Navio2Sensor() | |
| 28 |
2/4✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
|
112 | : Imu(typeStatic()) |
| 29 | { | ||
| 30 | LOG_TRACE("{}: called", name); | ||
| 31 | |||
| 32 | 112 | _onlyRealTime = true; | |
| 33 | 112 | _hasConfig = true; | |
| 34 | 112 | _guiConfigDefaultWindowSize = { 295, 92 }; | |
| 35 | |||
| 36 |
4/8✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 112 times.
✓ Branch 10 taken 112 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
448 | nm::CreateOutputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }); |
| 37 | 224 | } | |
| 38 | |||
| 39 | 224 | NAV::Navio2Sensor::~Navio2Sensor() | |
| 40 | { | ||
| 41 | LOG_TRACE("{}: called", nameId()); | ||
| 42 | 224 | } | |
| 43 | |||
| 44 | 224 | std::string NAV::Navio2Sensor::typeStatic() | |
| 45 | { | ||
| 46 |
1/2✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
|
448 | return "Navio2Sensor"; |
| 47 | } | ||
| 48 | |||
| 49 | ✗ | std::string NAV::Navio2Sensor::type() const | |
| 50 | { | ||
| 51 | ✗ | return typeStatic(); | |
| 52 | } | ||
| 53 | |||
| 54 | 112 | std::string NAV::Navio2Sensor::category() | |
| 55 | { | ||
| 56 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Provider"; |
| 57 | } | ||
| 58 | |||
| 59 | ✗ | void NAV::Navio2Sensor::guiConfig() | |
| 60 | { | ||
| 61 | ✗ | if (auto imuType = static_cast<int>(_imuType); | |
| 62 | ✗ | ImGui::Combo("IMU", &imuType, "MPU9250\0LSM9DS1\0\0")) | |
| 63 | { | ||
| 64 | ✗ | _imuType = static_cast<decltype(_imuType)>(imuType); | |
| 65 | ✗ | LOG_DEBUG("{}: IMU changed to {}", nameId(), _imuType ? "LSM9DS1" : "MPU9250"); | |
| 66 | ✗ | flow::ApplyChanges(); | |
| 67 | ✗ | doDeinitialize(); | |
| 68 | } | ||
| 69 | |||
| 70 | ✗ | if (ImGui::SliderInt("Frequency", &_outputFrequency, 1, 200, "%d Hz")) | |
| 71 | { | ||
| 72 | ✗ | LOG_DEBUG("{}: Frequency changed to {}", nameId(), _outputFrequency); | |
| 73 | ✗ | flow::ApplyChanges(); | |
| 74 | ✗ | doDeinitialize(); | |
| 75 | } | ||
| 76 | |||
| 77 | ✗ | Imu::guiConfig(); | |
| 78 | ✗ | } | |
| 79 | |||
| 80 | ✗ | [[nodiscard]] json NAV::Navio2Sensor::save() const | |
| 81 | { | ||
| 82 | LOG_TRACE("{}: called", nameId()); | ||
| 83 | |||
| 84 | ✗ | json j; | |
| 85 | |||
| 86 | ✗ | j["Frequency"] = _outputFrequency; | |
| 87 | ✗ | j["Imu"] = Imu::save(); | |
| 88 | |||
| 89 | ✗ | return j; | |
| 90 | ✗ | } | |
| 91 | |||
| 92 | ✗ | void NAV::Navio2Sensor::restore(json const& j) | |
| 93 | { | ||
| 94 | LOG_TRACE("{}: called", nameId()); | ||
| 95 | |||
| 96 | ✗ | if (j.contains("Frequency")) | |
| 97 | { | ||
| 98 | ✗ | j.at("Frequency").get_to(_outputFrequency); | |
| 99 | } | ||
| 100 | ✗ | if (j.contains("Imu")) | |
| 101 | { | ||
| 102 | ✗ | Imu::restore(j.at("Imu")); | |
| 103 | } | ||
| 104 | ✗ | } | |
| 105 | |||
| 106 | ✗ | bool NAV::Navio2Sensor::resetNode() | |
| 107 | { | ||
| 108 | ✗ | return true; | |
| 109 | } | ||
| 110 | |||
| 111 | ✗ | bool NAV::Navio2Sensor::initialize() | |
| 112 | { | ||
| 113 | LOG_TRACE("{} ({}): called", nameId(), _imuType ? "LSM9DS1" : "MPU9250"); | ||
| 114 | |||
| 115 | #if !__APPLE__ && !defined(WIN32) && !defined(_WIN32) && !defined(__WIN32) | ||
| 116 | ✗ | if (_imuType == ImuType::MPU) | |
| 117 | { | ||
| 118 | ✗ | _sensor = std::make_unique<MPU9250>(); | |
| 119 | } | ||
| 120 | else // ImuType::LSM | ||
| 121 | { | ||
| 122 | ✗ | _sensor = std::make_unique<LSM9DS1>(); | |
| 123 | } | ||
| 124 | |||
| 125 | ✗ | if (!_sensor->probe()) | |
| 126 | { | ||
| 127 | ✗ | LOG_ERROR("{} ({}): Sensor not enabled", nameId(), _imuType ? "LSM9DS1" : "MPU9250"); | |
| 128 | ✗ | return false; | |
| 129 | } | ||
| 130 | ✗ | _sensor->initialize(); | |
| 131 | #else | ||
| 132 | LOG_ERROR("{} ({}): MacOS is not supported by the Navio2 Node", nameId(), _imuType ? "LSM9DS1" : "MPU9250"); | ||
| 133 | return false; | ||
| 134 | #endif | ||
| 135 | |||
| 136 | ✗ | int outputInterval = static_cast<int>(1.0 / static_cast<double>(_outputFrequency) * 1000.0); | |
| 137 | ✗ | _startTime = std::chrono::steady_clock::now(); | |
| 138 | ✗ | _timer.start(outputInterval, readImuThread, this); | |
| 139 | |||
| 140 | ✗ | return true; | |
| 141 | } | ||
| 142 | |||
| 143 | ✗ | void NAV::Navio2Sensor::deinitialize() | |
| 144 | { | ||
| 145 | LOG_TRACE("{} ({}): called", nameId(), _imuType ? "LSM9DS1" : "MPU9250"); | ||
| 146 | |||
| 147 | ✗ | if (_timer.is_running()) | |
| 148 | { | ||
| 149 | ✗ | _timer.stop(); | |
| 150 | } | ||
| 151 | |||
| 152 | #if !__APPLE__ && !defined(WIN32) && !defined(_WIN32) && !defined(__WIN32) | ||
| 153 | ✗ | _sensor.reset(); | |
| 154 | #endif | ||
| 155 | ✗ | } | |
| 156 | |||
| 157 | // void NAV::Navio2Sensor::readImuThread() | ||
| 158 | ✗ | void NAV::Navio2Sensor::readImuThread(void* userData) | |
| 159 | { | ||
| 160 | ✗ | auto* navio = static_cast<Navio2Sensor*>(userData); | |
| 161 | ✗ | auto obs = std::make_shared<ImuObs>(navio->_imuPos); | |
| 162 | |||
| 163 | ✗ | auto currentTime = std::chrono::steady_clock::now(); | |
| 164 | #if !__APPLE__ && !defined(WIN32) && !defined(_WIN32) && !defined(__WIN32) | ||
| 165 | ✗ | navio->_sensor->update(); | |
| 166 | |||
| 167 | ✗ | navio->_sensor->read_accelerometer(&navio->_ax, &navio->_ay, &navio->_az); | |
| 168 | ✗ | navio->_sensor->read_gyroscope(&navio->_gx, &navio->_gy, &navio->_gz); | |
| 169 | ✗ | navio->_sensor->read_magnetometer(&navio->_mx, &navio->_my, &navio->_mz); | |
| 170 | |||
| 171 | ✗ | obs->temperature = navio->_sensor->read_temperature(); | |
| 172 | #endif | ||
| 173 | |||
| 174 | ✗ | obs->p_acceleration = { navio->_ax, navio->_ay, navio->_az }; | |
| 175 | ✗ | obs->p_angularRate = { navio->_gx, navio->_gy, navio->_gz }; | |
| 176 | |||
| 177 | ✗ | if (navio->_imuType == ImuType::LSM) | |
| 178 | { | ||
| 179 | ✗ | obs->p_magneticField.emplace(navio->_mx, navio->_my, navio->_mz); | |
| 180 | // constexpr double uT2Gauss = 1.0 / 100.0; | ||
| 181 | // obs->p_magneticField.value() *= uT2Gauss; | ||
| 182 | } | ||
| 183 | |||
| 184 | ✗ | std::chrono::nanoseconds diff = currentTime - navio->_startTime; | |
| 185 | ✗ | obs->timeSinceStartup = diff.count(); | |
| 186 | |||
| 187 | LOG_DATA("DATA({}): {}, {}°C, a=({}, {}, {})", navio->name, obs->timeSinceStartup.value(), obs->temperature.value(), | ||
| 188 | navio->_ax, navio->_ay, navio->_az); | ||
| 189 | |||
| 190 | ✗ | if (InsTime currentTime = util::time::GetCurrentInsTime(); | |
| 191 | ✗ | !currentTime.empty()) | |
| 192 | { | ||
| 193 | ✗ | obs->insTime = currentTime; | |
| 194 | } | ||
| 195 | ✗ | navio->invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs); | |
| 196 | ✗ | } | |
| 197 |