INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProvider/IMU/Sensors/Navio2Sensor.cpp
Date: 2025-07-19 10:51:51
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/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 114 NAV::Navio2Sensor::Navio2Sensor()
28
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 : Imu(typeStatic())
29 {
30 LOG_TRACE("{}: called", name);
31
32 114 _onlyRealTime = true;
33 114 _hasConfig = true;
34 114 _guiConfigDefaultWindowSize = { 295, 92 };
35
36
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 nm::CreateOutputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() });
37 228 }
38
39 228 NAV::Navio2Sensor::~Navio2Sensor()
40 {
41 LOG_TRACE("{}: called", nameId());
42 228 }
43
44 228 std::string NAV::Navio2Sensor::typeStatic()
45 {
46
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 456 return "Navio2Sensor";
47 }
48
49 ✗ std::string NAV::Navio2Sensor::type() const
50 {
51 ✗ return typeStatic();
52 }
53
54 114 std::string NAV::Navio2Sensor::category()
55 {
56
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 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 #if !__APPLE__ && !defined(WIN32) && !defined(_WIN32) && !defined(__WIN32)
164 ✗ navio->_sensor->update();
165
166 ✗ navio->_sensor->read_accelerometer(&navio->_ax, &navio->_ay, &navio->_az);
167 ✗ navio->_sensor->read_gyroscope(&navio->_gx, &navio->_gy, &navio->_gz);
168 ✗ navio->_sensor->read_magnetometer(&navio->_mx, &navio->_my, &navio->_mz);
169
170 ✗ obs->temperature = navio->_sensor->read_temperature();
171 #endif
172
173 ✗ obs->p_acceleration = { navio->_ax, navio->_ay, navio->_az };
174 ✗ obs->p_angularRate = { navio->_gx, navio->_gy, navio->_gz };
175
176 ✗ if (navio->_imuType == ImuType::LSM)
177 {
178 ✗ obs->p_magneticField.emplace(navio->_mx, navio->_my, navio->_mz);
179 // constexpr double uT2Gauss = 1.0 / 100.0;
180 // obs->p_magneticField.value() *= uT2Gauss;
181 }
182
183 LOG_DATA("DATA({}): {}°C, a=({}, {}, {})", navio->name, obs->temperature.value(),
184 navio->_ax, navio->_ay, navio->_az);
185
186 ✗ if (InsTime currentTime = util::time::GetCurrentInsTime();
187 ✗ !currentTime.empty())
188 {
189 ✗ obs->insTime = currentTime;
190 }
191 ✗ navio->invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
192 ✗ }
193