INSTINCT Code Coverage Report

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