INSTINCT Code Coverage Report

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