INSTINCT Code Coverage Report

Directory: src/
File: Nodes/State/PosVelAttInitializer.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 97 465 20.9%
Functions: 9 20 45.0%
Branches: 91 1112 8.2%
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 "PosVelAttInitializer.hpp"
10 #include "NodeRegistry.hpp"
11
12 #include "util/Logger.hpp"
13
14 #include "internal/FlowManager.hpp"
15
16 #include "internal/gui/widgets/EnumCombo.hpp"
17 #include "internal/gui/widgets/HelpMarker.hpp"
18 #include "internal/gui/widgets/imgui_ex.hpp"
19 #include "internal/gui/NodeEditorApplication.hpp"
20
21 #include "util/Time/TimeBase.hpp"
22 #include "util/Vendor/Ublox/UbloxTypes.hpp"
23 #include "Navigation/INS/Functions.hpp"
24 #include "Navigation/Transformations/CoordinateFrames.hpp"
25 #include "Navigation/Transformations/Units.hpp"
26
27 #include "NodeData/State/PosVelAtt.hpp"
28 #include "NodeData/GNSS/SppSolution.hpp"
29
30 #include <limits>
31
32 115 NAV::PosVelAttInitializer::PosVelAttInitializer()
33
9/18
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 115 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 115 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 115 times.
✗ Branch 16 not taken.
✓ Branch 21 taken 115 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 115 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 115 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 115 times.
✗ Branch 31 not taken.
 115 : Node(typeStatic())
34 {
35 LOG_TRACE("{}: called", name);
36
37 115 _hasConfig = true;
38 115 _guiConfigDefaultWindowSize = { 345, 342 };
39
40
4/8
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 115 times.
✓ Branch 10 taken 115 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 460 CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
41
42
1/2
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
 115 updatePins();
43 230 }
44
45 232 NAV::PosVelAttInitializer::~PosVelAttInitializer()
46 {
47 LOG_TRACE("{}: called", nameId());
48 232 }
49
50 229 std::string NAV::PosVelAttInitializer::typeStatic()
51 {
52
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 458 return "PosVelAttInitializer";
53 }
54
55 ✗ std::string NAV::PosVelAttInitializer::type() const
56 {
57 ✗ return typeStatic();
58 }
59
60 114 std::string NAV::PosVelAttInitializer::category()
61 {
62
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "State";
63 }
64
65 ✗ void NAV::PosVelAttInitializer::guiConfig()
66 {
67 ✗ if (_inputPinIdxIMU >= 0 && inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked()
68 ✗ && !(_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)))
69 {
70 ✗ ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio());
71 ✗ if (ImGui::InputDouble(fmt::format("Initialization Duration Attitude##{}", size_t(id)).c_str(), &_initDuration, 0.0, 0.0, "%.3f s"))
72 {
73 ✗ flow::ApplyChanges();
74 }
75 }
76
77 ✗ if (_inputPinIdxIMU >= 0 && inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked()
78 ✗ && _inputPinIdxGNSS >= 0 && inputPins.at(static_cast<size_t>(_inputPinIdxGNSS)).isPinLinked()
79 ✗ && !(_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)))
80 {
81 ✗ ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio());
82 ✗ if (gui::widgets::EnumCombo(fmt::format("Attitude Init Source##{}", size_t(id)).c_str(), _attitudeMode))
83 {
84 ✗ LOG_DEBUG("{}: Attitude Init Source changed to {}", nameId(), to_string(_attitudeMode));
85 ✗ flow::ApplyChanges();
86 }
87 }
88
89 ✗ if (ImGui::BeginTable(fmt::format("Initialized State##{}", size_t(id)).c_str(),
90 ✗ 4, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
91 {
92 ✗ ImGui::TableSetupColumn("");
93 ✗ ImGui::TableSetupColumn("");
94 ✗ ImGui::TableSetupColumn("Threshold");
95 ✗ ImGui::TableSetupColumn("Accuracy");
96 ✗ ImGui::TableHeadersRow();
97
98 /* -------------------------------------------------------------------------------------------------------- */
99 /* Position */
100 /* -------------------------------------------------------------------------------------------------------- */
101
102 ✗ ImGui::TableNextColumn();
103 ✗ ImGui::Text("Position");
104 ✗ ImGui::TableNextColumn();
105 ✗ float size = 7.0F;
106 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F,
107 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
108 size,
109 ✗ _posVelAttInitialized.at(0) || _overridePosition
110 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
111 : ImColor(255.0F, 0.0F, 0.0F));
112 ✗ ImGui::Selectable(("##determinePosition" + std::to_string(size_t(id))).c_str(),
113 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(1.6F * size, 0.0F));
114 ✗ if (ImGui::IsItemHovered())
115 {
116 ✗ ImGui::SetTooltip("%s", _posVelAttInitialized.at(0) || _overridePosition ? "Successfully Initialized" : "To be initialized");
117 }
118 ✗ ImGui::TableNextColumn();
119 ✗ ImGui::SetNextItemWidth(-FLT_MIN);
120 ✗ if (ImGui::DragDouble(("##positionAccuracyThreshold" + std::to_string(size_t(id))).c_str(),
121 &_positionAccuracyThreshold, 0.1F, 0.0, 1000.0, "%.1f cm"))
122 {
123 ✗ flow::ApplyChanges();
124 }
125 ✗ ImGui::TableNextColumn();
126 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F,
127 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
128 size,
129 ✗ _lastPositionAccuracy.at(0) <= _positionAccuracyThreshold
130 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
131 : ImColor(255.0F, 0.0F, 0.0F));
132 ✗ ImGui::Selectable(("##lastPositionAccuracy.at(0)" + std::to_string(size_t(id))).c_str(),
133 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F));
134 ✗ if (ImGui::IsItemHovered())
135 {
136 ✗ ImGui::SetTooltip("Last: %.3f cm", _lastPositionAccuracy.at(0));
137 }
138 ✗ ImGui::SameLine();
139 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F,
140 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
141 size,
142 ✗ _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold
143 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
144 : ImColor(255.0F, 0.0F, 0.0F));
145 ✗ ImGui::Selectable(("##lastPositionAccuracy.at(1)" + std::to_string(size_t(id))).c_str(),
146 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F));
147 ✗ if (ImGui::IsItemHovered())
148 {
149 ✗ ImGui::SetTooltip("Last: %.3f cm", _lastPositionAccuracy.at(1));
150 }
151 ✗ ImGui::SameLine();
152 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F,
153 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
154 size,
155 ✗ _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold
156 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
157 : ImColor(255.0F, 0.0F, 0.0F));
158 ✗ ImGui::Selectable(("##lastPositionAccuracy.at(2)" + std::to_string(size_t(id))).c_str(),
159 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F));
160 ✗ if (ImGui::IsItemHovered())
161 {
162 ✗ ImGui::SetTooltip("Last: %.3f cm", _lastPositionAccuracy.at(2));
163 }
164
165 /* -------------------------------------------------------------------------------------------------------- */
166 /* Velocity */
167 /* -------------------------------------------------------------------------------------------------------- */
168
169 ✗ ImGui::TableNextColumn();
170 ✗ ImGui::Text("Velocity");
171 ✗ ImGui::TableNextColumn();
172 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F,
173 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
174 size,
175 ✗ _posVelAttInitialized.at(1) || _overrideVelocity != VelocityOverride::OFF
176 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
177 : ImColor(255.0F, 0.0F, 0.0F));
178 ✗ ImGui::Selectable(("##determineVelocity" + std::to_string(size_t(id))).c_str(),
179 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(1.6F * size, 0.0F));
180 ✗ if (ImGui::IsItemHovered())
181 {
182 ✗ ImGui::SetTooltip("%s", _posVelAttInitialized.at(1) || _overrideVelocity != VelocityOverride::OFF
183 ? "Successfully Initialized"
184 : "To be initialized");
185 }
186 ✗ ImGui::TableNextColumn();
187 ✗ ImGui::SetNextItemWidth(-FLT_MIN);
188 ✗ if (ImGui::DragDouble(("##velocityAccuracyThreshold" + std::to_string(size_t(id))).c_str(),
189 &_velocityAccuracyThreshold, 1.0F, 0.0, 1000.0, "%.0f cm/s"))
190 {
191 ✗ flow::ApplyChanges();
192 }
193 ✗ ImGui::TableNextColumn();
194 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F,
195 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
196 size,
197 ✗ _lastVelocityAccuracy.at(0) <= _velocityAccuracyThreshold
198 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
199 : ImColor(255.0F, 0.0F, 0.0F));
200 ✗ ImGui::Selectable(("##lastVelocityAccuracy.at(0)" + std::to_string(size_t(id))).c_str(),
201 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F));
202 ✗ if (ImGui::IsItemHovered())
203 {
204 ✗ ImGui::SetTooltip("Last: %.3f cm", _lastVelocityAccuracy.at(0));
205 }
206 ✗ ImGui::SameLine();
207 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F,
208 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
209 size,
210 ✗ _lastVelocityAccuracy.at(1) <= _velocityAccuracyThreshold
211 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
212 : ImColor(255.0F, 0.0F, 0.0F));
213 ✗ ImGui::Selectable(("##lastVelocityAccuracy.at(1)" + std::to_string(size_t(id))).c_str(),
214 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F));
215 ✗ if (ImGui::IsItemHovered())
216 {
217 ✗ ImGui::SetTooltip("Last: %.3f cm", _lastVelocityAccuracy.at(1));
218 }
219 ✗ ImGui::SameLine();
220 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F,
221 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
222 size,
223 ✗ _lastVelocityAccuracy.at(2) <= _velocityAccuracyThreshold
224 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
225 : ImColor(255.0F, 0.0F, 0.0F));
226 ✗ ImGui::Selectable(("##lastVelocityAccuracy.at(2)" + std::to_string(size_t(id))).c_str(),
227 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F));
228 ✗ if (ImGui::IsItemHovered())
229 {
230 ✗ ImGui::SetTooltip("Last: %.3f cm", _lastVelocityAccuracy.at(2));
231 }
232
233 /* -------------------------------------------------------------------------------------------------------- */
234 /* Attitude */
235 /* -------------------------------------------------------------------------------------------------------- */
236
237 ✗ ImGui::TableNextColumn();
238 ✗ ImGui::Text("Attitude");
239 ✗ ImGui::TableNextColumn();
240 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F,
241 ✗ ImGui::GetCursorScreenPos().y + size * 1.4F),
242 size,
243 ✗ _posVelAttInitialized.at(2) || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))
244 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
245 : ImColor(255.0F, 0.0F, 0.0F));
246 ✗ ImGui::Selectable(("##determineAttitude" + std::to_string(size_t(id))).c_str(),
247 ✗ false, ImGuiSelectableFlags_Disabled, ImVec2(1.6F * size, 0.0F));
248 ✗ if (ImGui::IsItemHovered())
249 {
250 ✗ ImGui::SetTooltip("%s", _posVelAttInitialized.at(2) || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))
251 ? "Successfully Initialized"
252 : "To be initialized");
253 }
254 ✗ ImGui::TableNextColumn();
255
256 ✗ ImGui::EndTable();
257 }
258
259 ✗ if (ImGui::Checkbox(fmt::format("Override Position##{}", size_t(id)).c_str(), &_overridePosition))
260 {
261 ✗ updatePins();
262 ✗ flow::ApplyChanges();
263 }
264
265 ✗ if (_overridePosition)
266 {
267 ✗ ImGui::Indent();
268 ✗ if (gui::widgets::PositionInput(fmt::format("Reference Frame##{}", size_t(id)).c_str(), _overridePositionValue,
269 ✗ gui::widgets::PositionInputLayout::SINGLE_COLUMN, 140 * gui::NodeEditorApplication::windowFontRatio()))
270 {
271 ✗ flow::ApplyChanges();
272 }
273 ✗ ImGui::Unindent();
274 }
275
276 ✗ ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio());
277 ✗ if (gui::widgets::EnumCombo(fmt::format("Override Velocity##{}", size_t(id)).c_str(), _overrideVelocity))
278 {
279 ✗ updatePins();
280 ✗ flow::ApplyChanges();
281 }
282 ✗ if (_overrideVelocity != VelocityOverride::OFF)
283 {
284 ✗ ImGui::Indent();
285
286 ✗ bool ned = _overrideVelocity == VelocityOverride::NED;
287 ✗ ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio());
288 ✗ if (ImGui::DragDouble(fmt::format("Velocity {} [m/s]##{}", ned ? "North" : "ECEF X", size_t(id)).c_str(), &_overrideVelocityValues(0),
289 1.0F, 0.0, 0.0, "%.4f"))
290 {
291 ✗ flow::ApplyChanges();
292 }
293 ✗ ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio());
294 ✗ if (ImGui::DragDouble(fmt::format("Velocity {} [m/s]##{}", ned ? "Eeast" : "ECEF Y", size_t(id)).c_str(), &_overrideVelocityValues(1),
295 1.0F, 0.0, 0.0, "%.4f"))
296 {
297 ✗ flow::ApplyChanges();
298 }
299 ✗ ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio());
300 ✗ if (ImGui::DragDouble(fmt::format("Velocity {} [m/s]##{}", ned ? "Down" : "ECEF Z", size_t(id)).c_str(), &_overrideVelocityValues(2),
301 1.0F, 0.0, 0.0, "%.4f"))
302 {
303 ✗ flow::ApplyChanges();
304 }
305
306 ✗ ImGui::Unindent();
307 }
308
309 ✗ if (ImGui::BeginTable(("Overrides##" + std::to_string(size_t(id))).c_str(),
310 ✗ 2, ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
311 {
312 ✗ ImGui::TableNextColumn();
313 ✗ if (ImGui::Checkbox(("Override Roll##" + std::to_string(size_t(id))).c_str(), &_overrideRollPitchYaw.at(0)))
314 {
315 ✗ updatePins();
316 ✗ flow::ApplyChanges();
317 }
318 ✗ ImGui::TableNextColumn();
319 ✗ if (_overrideRollPitchYaw.at(0))
320 {
321 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
322 ✗ if (ImGui::DragDouble(("##overrideValuesRollPitchYaw.at(0)" + std::to_string(size_t(id))).c_str(),
323 ✗ &_overrideRollPitchYawValues.at(0), 1.0F, -180.0, 180.0, "%.3f °"))
324 {
325 ✗ flow::ApplyChanges();
326 }
327 }
328
329 ✗ ImGui::TableNextColumn();
330 ✗ if (ImGui::Checkbox(("Override Pitch##" + std::to_string(size_t(id))).c_str(), &_overrideRollPitchYaw.at(1)))
331 {
332 ✗ updatePins();
333 ✗ flow::ApplyChanges();
334 }
335 ✗ ImGui::TableNextColumn();
336 ✗ if (_overrideRollPitchYaw.at(1))
337 {
338 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
339 ✗ if (ImGui::DragDouble(("##overrideValuesRollPitchYaw.at(1)" + std::to_string(size_t(id))).c_str(),
340 ✗ &_overrideRollPitchYawValues.at(1), 1.0F, -90.0, 90.0, "%.3f °"))
341 {
342 ✗ flow::ApplyChanges();
343 }
344 }
345
346 ✗ ImGui::TableNextColumn();
347 ✗ if (ImGui::Checkbox(("Override Yaw##" + std::to_string(size_t(id))).c_str(), &_overrideRollPitchYaw.at(2)))
348 {
349 ✗ updatePins();
350 ✗ flow::ApplyChanges();
351 }
352 ✗ ImGui::TableNextColumn();
353 ✗ if (_overrideRollPitchYaw.at(2))
354 {
355 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
356 ✗ if (ImGui::DragDouble(("##overrideValuesRollPitchYaw.at(2)" + std::to_string(size_t(id))).c_str(),
357 ✗ &_overrideRollPitchYawValues.at(2), 1.0, -180.0, 180.0, "%.3f °"))
358 {
359 ✗ flow::ApplyChanges();
360 }
361 }
362
363 ✗ ImGui::EndTable();
364 }
365
366 ✗ if (_overridePosition && _overrideVelocity != VelocityOverride::OFF
367 ✗ && _overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2])
368 {
369 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
370 ✗ if (ImGui::TreeNode(fmt::format("Init Time##{}", size_t(id)).c_str()))
371 {
372 ✗ if (gui::widgets::TimeEdit(fmt::format("Init Time Edit {}", size_t(id)).c_str(), _initTime, _initTimeEditFormat))
373 {
374 ✗ LOG_DEBUG("{}: initTime changed to {}", nameId(), _initTime);
375 ✗ flow::ApplyChanges();
376 }
377 ✗ if (ImGui::Button("Reset"))
378 {
379 ✗ _initTime = InsTime(InsTime_GPSweekTow(0, 0, 0));
380 ✗ flow::ApplyChanges();
381 }
382 ✗ ImGui::TreePop();
383 }
384 }
385 ✗ }
386
387 ✗ [[nodiscard]] json NAV::PosVelAttInitializer::save() const
388 {
389 LOG_TRACE("{}: called", nameId());
390
391 ✗ json j;
392
393 ✗ j["initDuration"] = _initDuration;
394 ✗ j["attitudeMode"] = _attitudeMode;
395 ✗ j["positionAccuracyThreshold"] = _positionAccuracyThreshold;
396 ✗ j["velocityAccuracyThreshold"] = _velocityAccuracyThreshold;
397 ✗ j["overridePosition"] = _overridePosition;
398 ✗ j["overridePositionValues"] = _overridePositionValue;
399 ✗ j["overrideVelocity"] = _overrideVelocity;
400 ✗ j["overrideVelocityValues"] = _overrideVelocityValues;
401 ✗ j["overrideRollPitchYaw"] = _overrideRollPitchYaw;
402 ✗ j["overrideRollPitchYawValues"] = _overrideRollPitchYawValues;
403 ✗ j["initTime"] = _initTime;
404 ✗ j["initTimeEditFormat"] = _initTimeEditFormat;
405
406 ✗ return j;
407 ✗ }
408
409 1 void NAV::PosVelAttInitializer::restore(json const& j)
410 {
411 LOG_TRACE("{}: called", nameId());
412
413
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("initDuration"))
414 {
415 1 j.at("initDuration").get_to(_initDuration);
416 }
417
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("attitudeMode"))
418 {
419 1 j.at("attitudeMode").get_to(_attitudeMode);
420 }
421
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("positionAccuracyThreshold"))
422 {
423 1 j.at("positionAccuracyThreshold").get_to(_positionAccuracyThreshold);
424 }
425
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("velocityAccuracyThreshold"))
426 {
427 1 j.at("velocityAccuracyThreshold").get_to(_velocityAccuracyThreshold);
428 }
429
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("overridePosition"))
430 {
431 1 j.at("overridePosition").get_to(_overridePosition);
432 }
433
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("overridePositionValues"))
434 {
435 1 j.at("overridePositionValues").get_to(_overridePositionValue);
436 }
437
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("overrideVelocity"))
438 {
439 1 j.at("overrideVelocity").get_to(_overrideVelocity);
440 }
441
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("overrideVelocityValues"))
442 {
443 1 j.at("overrideVelocityValues").get_to(_overrideVelocityValues);
444 }
445
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("overrideRollPitchYaw"))
446 {
447 1 j.at("overrideRollPitchYaw").get_to(_overrideRollPitchYaw);
448 }
449
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("overrideRollPitchYawValues"))
450 {
451 1 j.at("overrideRollPitchYawValues").get_to(_overrideRollPitchYawValues);
452 }
453
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("initTime"))
454 {
455 1 j.at("initTime").get_to(_initTime);
456 }
457
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("initTimeEditFormat"))
458 {
459 1 j.at("initTimeEditFormat").get_to(_initTimeEditFormat);
460 }
461 1 updatePins();
462 1 }
463
464 3 bool NAV::PosVelAttInitializer::initialize()
465 {
466 LOG_TRACE("{}: called", nameId());
467
468 3 _startTime.reset();
469
470 3 _countAveragedAttitude = 0.0;
471
472 3 _lastPositionAccuracy = { std::numeric_limits<double>::infinity(),
473 std::numeric_limits<double>::infinity(),
474 std::numeric_limits<double>::infinity() };
475 3 _lastVelocityAccuracy = { std::numeric_limits<double>::infinity(),
476 std::numeric_limits<double>::infinity(),
477 std::numeric_limits<double>::infinity() };
478
479 3 _posVelAttInitialized = { false, false, false, false };
480
481
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (!_overridePosition || _overrideVelocity == VelocityOverride::OFF
482
5/10
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 3 times.
 6 || !_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2])
483 {
484 ✗ _initTime = InsTime(InsTime_GPSweekTow(0, 0, 0));
485 }
486
487 3 return true;
488 }
489
490 1 void NAV::PosVelAttInitializer::deinitialize()
491 {
492 LOG_TRACE("{}: called", nameId());
493 1 }
494
495 116 void NAV::PosVelAttInitializer::updatePins()
496 {
497
7/10
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 115 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 115 times.
 116 if (_overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2] && _inputPinIdxIMU >= 0)
498 {
499 1 DeleteInputPin(static_cast<size_t>(_inputPinIdxIMU));
500 1 _inputPinIdxIMU = -1;
501 1 _inputPinIdxGNSS--;
502 }
503
3/10
✗ Branch 1 not taken.
✓ Branch 2 taken 115 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 115 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 115 times.
✗ Branch 12 not taken.
 115 else if ((!_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2]) && _inputPinIdxIMU < 0)
504 {
505
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateInputPin("ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }, &PosVelAttInitializer::receiveImuObs, nullptr, 0, 0);
506 115 _inputPinIdxIMU = 0;
507
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 115 times.
 115 if (_inputPinIdxGNSS >= 0)
508 {
509 ✗ _inputPinIdxGNSS++;
510 }
511 }
512
513 232 if (_overridePosition
514
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 && _overrideVelocity != VelocityOverride::OFF
515
8/12
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 115 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✓ Branch 14 taken 115 times.
 117 && _overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2] && _inputPinIdxGNSS >= 0)
516 {
517 1 DeleteInputPin(static_cast<size_t>(_inputPinIdxGNSS));
518 1 _inputPinIdxGNSS = -1;
519 }
520 ✗ else if ((!_overridePosition || _overrideVelocity == VelocityOverride::OFF
521 ✗ || !_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2])
522
3/6
✗ Branch 0 not taken.
✓ Branch 1 taken 115 times.
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
 115 && _inputPinIdxGNSS < 0)
523 {
524
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 575 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 805 CreateInputPin("PosVelAttInit", Pin::Type::Flow,
525 { NAV::UbloxObs::type(), NAV::RtklibPosObs::type(), NAV::PosVelAtt::type(), NAV::PosVel::type(), NAV::Pos::type() },
526 &PosVelAttInitializer::receiveGnssObs, nullptr, 1);
527 115 _inputPinIdxGNSS = static_cast<int>(inputPins.size()) - 1;
528 }
529
530
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 115 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 116 if (_inputPinIdxGNSS < 0 && _inputPinIdxIMU < 0)
531 {
532 1 outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<OutputPin::PollDataFunc>(&PosVelAttInitializer::pollPVASolution);
533 }
534 else
535 {
536 115 outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<void*>(nullptr);
537 }
538 346 }
539
540 ✗ void NAV::PosVelAttInitializer::finalizeInit()
541 {
542 ✗ if (!_posVelAttInitialized.at(3)
543 ✗ && (_posVelAttInitialized.at(0) || _overridePosition)
544 ✗ && (_posVelAttInitialized.at(1) || _overrideVelocity != VelocityOverride::OFF)
545 ✗ && (_posVelAttInitialized.at(2) || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))))
546 {
547 ✗ for (auto& inputPin : inputPins)
548 {
549 ✗ inputPin.queueBlocked = true;
550 ✗ inputPin.queue.clear();
551 }
552 ✗ _posVelAttInitialized.at(3) = true;
553
554 ✗ if (_overridePosition)
555 {
556 ✗ _e_initPosition = _overridePositionValue.e_position;
557 }
558 ✗ Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(_e_initPosition);
559 ✗ LOG_INFO("{}: Position initialized to Lat {:3.12f} [°], Lon {:3.12f} [°], Alt {:4.3f} [m]", nameId(),
560 rad2deg(lla_position.x()),
561 rad2deg(lla_position.y()),
562 lla_position.z());
563
564 ✗ if (_overrideVelocity == VelocityOverride::NED)
565 {
566 ✗ _n_initVelocity = _overrideVelocityValues;
567 }
568 ✗ else if (_overrideVelocity == VelocityOverride::ECEF)
569 {
570 ✗ _n_initVelocity = trafo::n_Quat_e(lla_position(0), lla_position(1)) * _overrideVelocityValues;
571 }
572
573 ✗ if (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))
574 {
575 ✗ _n_Quat_b_init = trafo::n_Quat_b(deg2rad(_overrideRollPitchYawValues.at(0)),
576 ✗ deg2rad(_overrideRollPitchYawValues.at(1)),
577 ✗ deg2rad(_overrideRollPitchYawValues.at(2)));
578 }
579
580 ✗ if (lla_position.z() < 0)
581 {
582 ✗ LOG_WARN("{}: Altitude has a value of {} which is below the ellipsoid height.", nameId(), lla_position.z());
583 }
584
585 ✗ LOG_INFO("{}: Velocity initialized to v_N {:3.5f} [m/s], v_E {:3.5f} [m/s], v_D {:3.5f} [m/s]", nameId(),
586 _n_initVelocity(0), _n_initVelocity(1), _n_initVelocity(2));
587
588 ✗ [[maybe_unused]] Eigen::Vector3d rollPitchYaw = trafo::quat2eulerZYX(_n_Quat_b_init);
589 ✗ LOG_INFO("{}: Attitude initialized to Roll {:3.5f} [°], Pitch {:3.5f} [°], Yaw {:3.4f} [°]", nameId(),
590 rad2deg(rollPitchYaw.x()),
591 rad2deg(rollPitchYaw.y()),
592 rad2deg(rollPitchYaw.z()));
593
594 ✗ auto posVelAtt = std::make_shared<PosVelAtt>();
595 ✗ posVelAtt->insTime = _initTime;
596 ✗ posVelAtt->setPosition_e(_e_initPosition);
597 ✗ posVelAtt->setVelocity_n(_n_initVelocity);
598 ✗ posVelAtt->setAttitude_n_Quat_b(_n_Quat_b_init);
599 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, posVelAtt);
600 ✗ }
601 ✗ else if (std::ranges::all_of(inputPins, [](const InputPin& inputPin) {
602 ✗ if (auto* connectedPin = inputPin.link.getConnectedPin())
603 {
604 ✗ return connectedPin->noMoreDataAvailable.load();
605 }
606 ✗ return !inputPin.isPinLinked();
607 }))
608 {
609 ✗ LOG_ERROR("{}: State Initialization failed. No more messages incoming to eventually receive a state. Please check which states got initialized and override the others.", nameId());
610 }
611 ✗ }
612
613 ✗ void NAV::PosVelAttInitializer::receiveImuObs(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
614 {
615 ✗ auto obs = std::static_pointer_cast<const ImuObs>(queue.extract_front());
616
617 ✗ if (_posVelAttInitialized.at(3)) { return; }
618 LOG_DATA("{}: receiveImuObs at time [{}]", nameId(), obs->insTime.toYMDHMS());
619
620 ✗ if (_startTime.empty()) { _startTime = obs->insTime; }
621
622 // Position and rotation information for conversion of IMU data from platform to body frame
623 ✗ const auto& imuPosition = obs->imuPos;
624
625 // Choose compenated data if available, otherwise uncompensated
626 ✗ if (!_overrideRollPitchYaw.at(2) && !obs->p_magneticField.has_value())
627 {
628 ✗ LOG_ERROR("No magnetometer data available. Please override the Yaw angle.");
629 ✗ return;
630 }
631 ✗ Eigen::Vector3d mag_p = obs->p_magneticField ? obs->p_magneticField.value() : Eigen::Vector3d::Zero();
632
633 // Calculate Magnetic Heading
634 ✗ const Eigen::Vector3d b_mag = imuPosition.b_quat_p() * mag_p;
635 ✗ auto magneticHeading = -std::atan2(b_mag.y(), b_mag.x());
636
637 // Calculate Roll and Pitch from gravity vector direction (only valid under static conditions)
638 ✗ const Eigen::Vector3d b_accel = imuPosition.b_quat_p() * obs->p_acceleration * -1;
639 ✗ auto roll = calcRollFromStaticAcceleration(b_accel);
640 ✗ auto pitch = calcPitchFromStaticAcceleration(b_accel);
641
642 // TODO: Determine Velocity first and if vehicle not static, initialize the attitude from velocity
643
644 // Average with previous attitude
645 ✗ _countAveragedAttitude++;
646 ✗ if (_countAveragedAttitude > 1)
647 {
648 ✗ _averagedAttitude.at(0) = (_averagedAttitude.at(0) * (_countAveragedAttitude - 1) + roll) / _countAveragedAttitude;
649 ✗ _averagedAttitude.at(1) = (_averagedAttitude.at(1) * (_countAveragedAttitude - 1) + pitch) / _countAveragedAttitude;
650 ✗ _averagedAttitude.at(2) = (_averagedAttitude.at(2) * (_countAveragedAttitude - 1) + magneticHeading) / _countAveragedAttitude;
651 }
652 else
653 {
654 ✗ _averagedAttitude.at(0) = roll;
655 ✗ _averagedAttitude.at(1) = pitch;
656 ✗ _averagedAttitude.at(2) = magneticHeading;
657 }
658
659 ✗ if ((_attitudeMode == AttitudeMode::BOTH || _attitudeMode == AttitudeMode::IMU
660 ✗ || _inputPinIdxGNSS < 0 || !inputPins.at(static_cast<size_t>(_inputPinIdxGNSS)).isPinLinked())
661 ✗ && (static_cast<double>((obs->insTime - _startTime).count()) >= _initDuration
662 ✗ || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))))
663 {
664 ✗ _n_Quat_b_init = trafo::n_Quat_b(_overrideRollPitchYaw.at(0) ? deg2rad(_overrideRollPitchYawValues.at(0)) : _averagedAttitude.at(0),
665 ✗ _overrideRollPitchYaw.at(1) ? deg2rad(_overrideRollPitchYawValues.at(1)) : _averagedAttitude.at(1),
666 ✗ _overrideRollPitchYaw.at(2) ? deg2rad(_overrideRollPitchYawValues.at(2)) : _averagedAttitude.at(2));
667
668 ✗ _initTime = obs->insTime;
669 ✗ _posVelAttInitialized.at(2) = true;
670 }
671
672 ✗ finalizeInit();
673 ✗ }
674
675 ✗ void NAV::PosVelAttInitializer::receiveGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx)
676 {
677 ✗ auto nodeData = queue.extract_front();
678
679 ✗ if (_posVelAttInitialized.at(3)) { return; }
680 LOG_DATA("{}: receiveGnssObs at time [{}]", nameId(), nodeData->insTime.toYMDHMS());
681
682 ✗ const auto* sourcePin = inputPins.at(pinIdx).link.getConnectedPin();
683
684 ✗ if (sourcePin->dataIdentifier.front() == UbloxObs::type())
685 {
686 ✗ receiveUbloxObs(std::static_pointer_cast<const UbloxObs>(nodeData));
687 }
688 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { PosVelAtt::type() }))
689 {
690 ✗ receivePosVelAttObs(std::static_pointer_cast<const PosVelAtt>(nodeData));
691 }
692 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { PosVel::type() }))
693 {
694 ✗ receivePosVelObs(std::static_pointer_cast<const PosVel>(nodeData));
695 }
696 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { Pos::type() }))
697 {
698 ✗ receivePosObs(std::static_pointer_cast<const Pos>(nodeData));
699 }
700
701 ✗ finalizeInit();
702 ✗ }
703
704 ✗ void NAV::PosVelAttInitializer::receiveUbloxObs(const std::shared_ptr<const UbloxObs>& obs)
705 {
706 ✗ if (obs->msgClass == vendor::ublox::UbxClass::UBX_CLASS_NAV)
707 {
708 ✗ auto msgId = static_cast<vendor::ublox::UbxNavMessages>(obs->msgId);
709 ✗ if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_ATT)
710 {
711 LOG_DATA("{}: UBX_NAV_ATT: Roll {}, Pitch {}, Heading {} [deg]", nameId(),
712 std::get<vendor::ublox::UbxNavAtt>(obs->data).roll * 1e-5,
713 std::get<vendor::ublox::UbxNavAtt>(obs->data).pitch * 1e-5,
714 std::get<vendor::ublox::UbxNavAtt>(obs->data).heading * 1e-5);
715 }
716 ✗ else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_POSECEF)
717 {
718 ✗ _lastPositionAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc);
719 ✗ _lastPositionAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc);
720 ✗ _lastPositionAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc);
721
722 ✗ if (_lastPositionAccuracy.at(0) <= _positionAccuracyThreshold
723 ✗ && _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold
724 ✗ && _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold)
725 {
726 ✗ _e_initPosition = Eigen::Vector3d(std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefX * 1e-2,
727 ✗ std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefY * 1e-2,
728 ✗ std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefZ * 1e-2);
729 ✗ _initTime = obs->insTime;
730
731 ✗ _posVelAttInitialized.at(0) = true;
732 }
733 }
734 ✗ else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_POSLLH)
735 {
736 ✗ _lastPositionAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).hAcc * 1e-1);
737 ✗ _lastPositionAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).hAcc * 1e-1);
738 ✗ _lastPositionAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).vAcc * 1e-1);
739
740 ✗ if (_lastPositionAccuracy.at(0) <= _positionAccuracyThreshold
741 ✗ && _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold
742 ✗ && _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold)
743 {
744 ✗ Eigen::Vector3d lla_position(deg2rad(std::get<vendor::ublox::UbxNavPosllh>(obs->data).lat * 1e-7),
745 ✗ deg2rad(std::get<vendor::ublox::UbxNavPosllh>(obs->data).lon * 1e-7),
746 ✗ std::get<vendor::ublox::UbxNavPosllh>(obs->data).height * 1e-3);
747
748 ✗ _e_initPosition = trafo::lla2ecef_WGS84(lla_position);
749 ✗ _initTime = obs->insTime;
750
751 ✗ _posVelAttInitialized.at(0) = true;
752 }
753 }
754 ✗ else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_VELNED)
755 {
756 ✗ _lastVelocityAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc);
757 ✗ _lastVelocityAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc);
758 ✗ _lastVelocityAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc);
759
760 ✗ if (_lastVelocityAccuracy.at(0) <= _velocityAccuracyThreshold
761 ✗ && _lastVelocityAccuracy.at(1) <= _velocityAccuracyThreshold
762 ✗ && _lastVelocityAccuracy.at(2) <= _velocityAccuracyThreshold)
763 {
764 ✗ _n_initVelocity = Eigen::Vector3d(std::get<vendor::ublox::UbxNavVelned>(obs->data).velN * 1e-2,
765 ✗ std::get<vendor::ublox::UbxNavVelned>(obs->data).velE * 1e-2,
766 ✗ std::get<vendor::ublox::UbxNavVelned>(obs->data).velD * 1e-2);
767 ✗ _initTime = obs->insTime;
768
769 ✗ _posVelAttInitialized.at(1) = true;
770 }
771 }
772 }
773 ✗ }
774
775 ✗ void NAV::PosVelAttInitializer::receivePosObs(const std::shared_ptr<const Pos>& obs)
776 {
777 ✗ _e_initPosition = obs->e_position();
778 ✗ _initTime = obs->insTime;
779 ✗ _posVelAttInitialized.at(0) = true;
780 ✗ }
781
782 ✗ void NAV::PosVelAttInitializer::receivePosVelObs(const std::shared_ptr<const PosVel>& obs)
783 {
784 ✗ receivePosObs(obs);
785
786 ✗ _n_initVelocity = obs->n_velocity();
787 ✗ _initTime = obs->insTime;
788 ✗ _posVelAttInitialized.at(1) = true;
789 ✗ }
790
791 ✗ void NAV::PosVelAttInitializer::receivePosVelAttObs(const std::shared_ptr<const PosVelAtt>& obs)
792 {
793 ✗ receivePosVelObs(obs);
794
795 ✗ if (_attitudeMode == AttitudeMode::BOTH || _attitudeMode == AttitudeMode::GNSS
796 ✗ || _inputPinIdxIMU < 0 || !inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked())
797 {
798 ✗ if (_overrideRollPitchYaw.at(0) || _overrideRollPitchYaw.at(1) || _overrideRollPitchYaw.at(2))
799 {
800 ✗ const Eigen::Vector3d rollPitchYaw = obs->rollPitchYaw();
801
802 ✗ _n_Quat_b_init = trafo::n_Quat_b(_overrideRollPitchYaw.at(0) ? deg2rad(_overrideRollPitchYawValues.at(0)) : rollPitchYaw(0),
803 ✗ _overrideRollPitchYaw.at(1) ? deg2rad(_overrideRollPitchYawValues.at(1)) : rollPitchYaw(1),
804 ✗ _overrideRollPitchYaw.at(2) ? deg2rad(_overrideRollPitchYawValues.at(2)) : rollPitchYaw(2));
805 }
806 else
807 {
808 ✗ _n_Quat_b_init = obs->n_Quat_b();
809 }
810 ✗ _initTime = obs->insTime;
811
812 ✗ _posVelAttInitialized.at(2) = true;
813 }
814 ✗ }
815
816 2 std::shared_ptr<const NAV::NodeData> NAV::PosVelAttInitializer::pollPVASolution()
817 {
818
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 2 if (_inputPinIdxIMU >= 0 || _inputPinIdxGNSS >= 0)
819 {
820 ✗ return nullptr;
821 }
822
823 2 int initCount = 0;
824
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (_overridePosition)
825 {
826 2 _e_initPosition = _overridePositionValue.e_position;
827 2 ++initCount;
828 }
829
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (_overrideVelocity == VelocityOverride::NED)
830 {
831 2 _n_initVelocity = _overrideVelocityValues;
832 2 ++initCount;
833 }
834 ✗ else if (_overrideVelocity == VelocityOverride::ECEF)
835 {
836 ✗ Eigen::Vector3d pos_lla = trafo::ecef2lla_WGS84(_e_initPosition);
837 ✗ _n_initVelocity = trafo::n_Quat_e(pos_lla(0), pos_lla(1)) * _overrideVelocityValues;
838 ✗ ++initCount;
839 }
840
841
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 2 if (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))
842 {
843
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 _n_Quat_b_init = trafo::n_Quat_b(deg2rad(_overrideRollPitchYawValues.at(0)),
844
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 deg2rad(_overrideRollPitchYawValues.at(1)),
845
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 deg2rad(_overrideRollPitchYawValues.at(2)));
846 2 ++initCount;
847 }
848
5/6
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 1 times.
 2 if (initCount == 3 && !_posVelAttInitialized.at(3))
849 {
850
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 auto posVelAtt = std::make_shared<PosVelAtt>();
851 1 posVelAtt->insTime = _initTime;
852
853
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 _posVelAttInitialized.at(3) = true;
854
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 posVelAtt->setPosition_e(_e_initPosition);
855
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 posVelAtt->setVelocity_n(_n_initVelocity);
856
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 posVelAtt->setAttitude_n_Quat_b(_n_Quat_b_init);
857
858
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, posVelAtt);
859 1 return posVelAtt;
860 1 }
861 1 return nullptr;
862 }
863