0.4.1
Loading...
Searching...
No Matches
PosVelAttInitializer.cpp
Go to the documentation of this file.
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"
15namespace 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
34NAV::PosVelAttInitializer::PosVelAttInitializer()
35 : Node(typeStatic())
36{
37 LOG_TRACE("{}: called", name);
38
39 _hasConfig = true;
40 _guiConfigDefaultWindowSize = { 345, 342 };
41
42 nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
43
44 updatePins();
45}
46
51
52std::string NAV::PosVelAttInitializer::typeStatic()
53{
54 return "PosVelAttInitializer";
55}
56
57std::string NAV::PosVelAttInitializer::type() const
58{
59 return typeStatic();
60}
61
62std::string NAV::PosVelAttInitializer::category()
63{
64 return "State";
65}
66
67void 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
411void 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
466bool NAV::PosVelAttInitializer::initialize()
467{
468 LOG_TRACE("{}: called", nameId());
469
470 _startTime.reset();
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
492void NAV::PosVelAttInitializer::deinitialize()
493{
494 LOG_TRACE("{}: called", nameId());
495}
496
497void NAV::PosVelAttInitializer::updatePins()
498{
499 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 else if ((!_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2]) && _inputPinIdxIMU < 0)
506 {
507 nm::CreateInputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }, &PosVelAttInitializer::receiveImuObs, nullptr, 0, 0);
508 _inputPinIdxIMU = 0;
509 if (_inputPinIdxGNSS >= 0)
510 {
511 _inputPinIdxGNSS++;
512 }
513 }
514
515 if (_overridePosition
516 && _overrideVelocity != VelocityOverride::OFF
517 && _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 && _inputPinIdxGNSS < 0)
525 {
526 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 _inputPinIdxGNSS = static_cast<int>(inputPins.size()) - 1;
530 }
531
532 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 outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<void*>(nullptr);
539 }
540}
541
542void 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
615void 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 (_startTime.empty()) { _startTime = obs->insTime; }
623
624 // Position and rotation information for conversion of IMU data from platform to body frame
625 const auto& imuPosition = obs->imuPos;
626
627 // Choose compenated data if available, otherwise uncompensated
628 if (!_overrideRollPitchYaw.at(2) && !obs->p_magneticField.has_value())
629 {
630 LOG_ERROR("No magnetometer data available. Please override the Yaw angle.");
631 return;
632 }
633 Eigen::Vector3d mag_p = obs->p_magneticField ? obs->p_magneticField.value() : Eigen::Vector3d::Zero();
634
635 // Calculate Magnetic Heading
636 const Eigen::Vector3d b_mag = imuPosition.b_quat_p() * mag_p;
637 auto magneticHeading = -std::atan2(b_mag.y(), b_mag.x());
638
639 // Calculate Roll and Pitch from gravity vector direction (only valid under static conditions)
640 const Eigen::Vector3d b_accel = imuPosition.b_quat_p() * obs->p_acceleration * -1;
641 auto roll = calcRollFromStaticAcceleration(b_accel);
642 auto pitch = calcPitchFromStaticAcceleration(b_accel);
643
644 // TODO: Determine Velocity first and if vehicle not static, initialize the attitude from velocity
645
646 // Average with previous attitude
647 _countAveragedAttitude++;
648 if (_countAveragedAttitude > 1)
649 {
650 _averagedAttitude.at(0) = (_averagedAttitude.at(0) * (_countAveragedAttitude - 1) + roll) / _countAveragedAttitude;
651 _averagedAttitude.at(1) = (_averagedAttitude.at(1) * (_countAveragedAttitude - 1) + pitch) / _countAveragedAttitude;
652 _averagedAttitude.at(2) = (_averagedAttitude.at(2) * (_countAveragedAttitude - 1) + magneticHeading) / _countAveragedAttitude;
653 }
654 else
655 {
656 _averagedAttitude.at(0) = roll;
657 _averagedAttitude.at(1) = pitch;
658 _averagedAttitude.at(2) = magneticHeading;
659 }
660
661 if ((_attitudeMode == AttitudeMode::BOTH || _attitudeMode == AttitudeMode::IMU
662 || _inputPinIdxGNSS < 0 || !inputPins.at(static_cast<size_t>(_inputPinIdxGNSS)).isPinLinked())
663 && (static_cast<double>((obs->insTime - _startTime).count()) >= _initDuration
664 || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))))
665 {
666 _n_Quat_b_init = trafo::n_Quat_b(_overrideRollPitchYaw.at(0) ? deg2rad(_overrideRollPitchYawValues.at(0)) : _averagedAttitude.at(0),
667 _overrideRollPitchYaw.at(1) ? deg2rad(_overrideRollPitchYawValues.at(1)) : _averagedAttitude.at(1),
668 _overrideRollPitchYaw.at(2) ? deg2rad(_overrideRollPitchYawValues.at(2)) : _averagedAttitude.at(2));
669
670 _initTime = obs->insTime;
671 _posVelAttInitialized.at(2) = true;
672 }
673
674 finalizeInit();
675}
676
677void NAV::PosVelAttInitializer::receiveGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx)
678{
679 auto nodeData = queue.extract_front();
680
681 if (_posVelAttInitialized.at(3)) { return; }
682 LOG_DATA("{}: receiveGnssObs at time [{}]", nameId(), nodeData->insTime.toYMDHMS());
683
684 const auto* sourcePin = inputPins.at(pinIdx).link.getConnectedPin();
685
686 if (sourcePin->dataIdentifier.front() == UbloxObs::type())
687 {
688 receiveUbloxObs(std::static_pointer_cast<const UbloxObs>(nodeData));
689 }
690 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { PosVelAtt::type() }))
691 {
692 receivePosVelAttObs(std::static_pointer_cast<const PosVelAtt>(nodeData));
693 }
694 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { PosVel::type() }))
695 {
696 receivePosVelObs(std::static_pointer_cast<const PosVel>(nodeData));
697 }
698 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { Pos::type() }))
699 {
700 receivePosObs(std::static_pointer_cast<const Pos>(nodeData));
701 }
702
703 finalizeInit();
704}
705
706void NAV::PosVelAttInitializer::receiveUbloxObs(const std::shared_ptr<const UbloxObs>& obs)
707{
708 if (obs->msgClass == vendor::ublox::UbxClass::UBX_CLASS_NAV)
709 {
710 auto msgId = static_cast<vendor::ublox::UbxNavMessages>(obs->msgId);
711 if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_ATT)
712 {
713 LOG_DATA("{}: UBX_NAV_ATT: Roll {}, Pitch {}, Heading {} [deg]", nameId(),
714 std::get<vendor::ublox::UbxNavAtt>(obs->data).roll * 1e-5,
715 std::get<vendor::ublox::UbxNavAtt>(obs->data).pitch * 1e-5,
716 std::get<vendor::ublox::UbxNavAtt>(obs->data).heading * 1e-5);
717 }
718 else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_POSECEF)
719 {
720 _lastPositionAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc);
721 _lastPositionAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc);
722 _lastPositionAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc);
723
724 if (_lastPositionAccuracy.at(0) <= _positionAccuracyThreshold
725 && _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold
726 && _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold)
727 {
728 _e_initPosition = Eigen::Vector3d(std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefX * 1e-2,
729 std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefY * 1e-2,
730 std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefZ * 1e-2);
731 _initTime = obs->insTime;
732
733 _posVelAttInitialized.at(0) = true;
734 }
735 }
736 else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_POSLLH)
737 {
738 _lastPositionAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).hAcc * 1e-1);
739 _lastPositionAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).hAcc * 1e-1);
740 _lastPositionAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).vAcc * 1e-1);
741
742 if (_lastPositionAccuracy.at(0) <= _positionAccuracyThreshold
743 && _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold
744 && _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold)
745 {
746 Eigen::Vector3d lla_position(deg2rad(std::get<vendor::ublox::UbxNavPosllh>(obs->data).lat * 1e-7),
747 deg2rad(std::get<vendor::ublox::UbxNavPosllh>(obs->data).lon * 1e-7),
748 std::get<vendor::ublox::UbxNavPosllh>(obs->data).height * 1e-3);
749
750 _e_initPosition = trafo::lla2ecef_WGS84(lla_position);
751 _initTime = obs->insTime;
752
753 _posVelAttInitialized.at(0) = true;
754 }
755 }
756 else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_VELNED)
757 {
758 _lastVelocityAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc);
759 _lastVelocityAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc);
760 _lastVelocityAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc);
761
762 if (_lastVelocityAccuracy.at(0) <= _velocityAccuracyThreshold
763 && _lastVelocityAccuracy.at(1) <= _velocityAccuracyThreshold
764 && _lastVelocityAccuracy.at(2) <= _velocityAccuracyThreshold)
765 {
766 _n_initVelocity = Eigen::Vector3d(std::get<vendor::ublox::UbxNavVelned>(obs->data).velN * 1e-2,
767 std::get<vendor::ublox::UbxNavVelned>(obs->data).velE * 1e-2,
768 std::get<vendor::ublox::UbxNavVelned>(obs->data).velD * 1e-2);
769 _initTime = obs->insTime;
770
771 _posVelAttInitialized.at(1) = true;
772 }
773 }
774 }
775}
776
777void NAV::PosVelAttInitializer::receivePosObs(const std::shared_ptr<const Pos>& obs)
778{
779 _e_initPosition = obs->e_position();
780 _initTime = obs->insTime;
781 _posVelAttInitialized.at(0) = true;
782}
783
784void NAV::PosVelAttInitializer::receivePosVelObs(const std::shared_ptr<const PosVel>& obs)
785{
786 receivePosObs(obs);
787
788 _n_initVelocity = obs->n_velocity();
789 _initTime = obs->insTime;
790 _posVelAttInitialized.at(1) = true;
791}
792
793void NAV::PosVelAttInitializer::receivePosVelAttObs(const std::shared_ptr<const PosVelAtt>& obs)
794{
795 receivePosVelObs(obs);
796
797 if (_attitudeMode == AttitudeMode::BOTH || _attitudeMode == AttitudeMode::GNSS
798 || _inputPinIdxIMU < 0 || !inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked())
799 {
800 if (_overrideRollPitchYaw.at(0) || _overrideRollPitchYaw.at(1) || _overrideRollPitchYaw.at(2))
801 {
802 const Eigen::Vector3d rollPitchYaw = obs->rollPitchYaw();
803
804 _n_Quat_b_init = trafo::n_Quat_b(_overrideRollPitchYaw.at(0) ? deg2rad(_overrideRollPitchYawValues.at(0)) : rollPitchYaw(0),
805 _overrideRollPitchYaw.at(1) ? deg2rad(_overrideRollPitchYawValues.at(1)) : rollPitchYaw(1),
806 _overrideRollPitchYaw.at(2) ? deg2rad(_overrideRollPitchYawValues.at(2)) : rollPitchYaw(2));
807 }
808 else
809 {
810 _n_Quat_b_init = obs->n_Quat_b();
811 }
812 _initTime = obs->insTime;
813
814 _posVelAttInitialized.at(2) = true;
815 }
816}
817
818std::shared_ptr<const NAV::NodeData> NAV::PosVelAttInitializer::pollPVASolution()
819{
820 if (_inputPinIdxIMU >= 0 || _inputPinIdxGNSS >= 0)
821 {
822 return nullptr;
823 }
824
825 int initCount = 0;
826 if (_overridePosition)
827 {
828 _e_initPosition = _overridePositionValue.e_position;
829 ++initCount;
830 }
831 if (_overrideVelocity == VelocityOverride::NED)
832 {
833 _n_initVelocity = _overrideVelocityValues;
834 ++initCount;
835 }
836 else if (_overrideVelocity == VelocityOverride::ECEF)
837 {
838 Eigen::Vector3d pos_lla = trafo::ecef2lla_WGS84(_e_initPosition);
839 _n_initVelocity = trafo::n_Quat_e(pos_lla(0), pos_lla(1)) * _overrideVelocityValues;
840 ++initCount;
841 }
842
843 if (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))
844 {
845 _n_Quat_b_init = trafo::n_Quat_b(deg2rad(_overrideRollPitchYawValues.at(0)),
846 deg2rad(_overrideRollPitchYawValues.at(1)),
847 deg2rad(_overrideRollPitchYawValues.at(2)));
848 ++initCount;
849 }
850 if (initCount == 3 && !_posVelAttInitialized.at(3))
851 {
852 auto posVelAtt = std::make_shared<PosVelAtt>();
853 posVelAtt->insTime = _initTime;
854
855 _posVelAttInitialized.at(3) = true;
856 posVelAtt->setPosition_e(_e_initPosition);
857 posVelAtt->setVelocity_n(_n_initVelocity);
858 posVelAtt->setAttitude_n_Quat_b(_n_Quat_b_init);
859
860 invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, posVelAtt);
861 return posVelAtt;
862 }
863 return nullptr;
864}
CoordinateFrames.hpp
Transformation collection.
EnumCombo.hpp
Combo representing an enumeration.
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:21
HelpMarker.hpp
Text Help Marker (?) with Tooltip.
Functions.hpp
Inertial Navigation Helper Functions.
Logger.hpp
Utility class for logging to console and file.
LOG_DEBUG
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
LOG_DATA
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
LOG_ERROR
#define LOG_ERROR
Error occurred, which stops part of the program to work, but not everything.
Definition Logger.hpp:73
LOG_WARN
#define LOG_WARN
Error occurred, but a fallback option exists and program continues to work normally.
Definition Logger.hpp:71
LOG_INFO
#define LOG_INFO
Info to the user on the state of the program.
Definition Logger.hpp:69
LOG_TRACE
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
NodeManager.hpp
Manages all Nodes.
NodeRegistry.hpp
Utility class which specifies available nodes.
PosVelAttInitializer.hpp
Position, Velocity, Attitude Initializer from GPS and IMU data.
PosVelAtt.hpp
Position, Velocity and Attitude Storage Class.
SppSolution.hpp
SPP Algorithm output.
TimeBase.hpp
Keeps track of the current real/simulation time.
UbloxTypes.hpp
Type Definitions for Ublox messages.
NAV::ImuObs::type
static std::string type()
Returns the type of the data class.
Definition ImuObs.hpp:33
NAV::InputPin
Input pins of nodes.
Definition Pin.hpp:491
NAV::InputPin::link
IncomingLink link
Info to identify the linked pin.
Definition Pin.hpp:704
NAV::InputPin::NodeDataQueue
TsDeque< std::shared_ptr< const NAV::NodeData > > NodeDataQueue
Node data queue type.
Definition Pin.hpp:707
NAV::InputPin::isPinLinked
bool isPinLinked() const
Checks if the pin is linked.
Definition Pin.cpp:355
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:410
NAV::Node::outputPins
std::vector< OutputPin > outputPins
List of output pins.
Definition Node.hpp:399
NAV::Node::Node
Node(std::string name)
Constructor.
Definition Node.cpp:30
NAV::Node::inputPins
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:397
NAV::Node::nameId
std::string nameId() const
Node name and id.
Definition Node.cpp:253
NAV::Node::name
std::string name
Name of the Node.
Definition Node.hpp:395
NAV::Node::invokeCallbacks
void invokeCallbacks(size_t portIndex, const std::shared_ptr< const NodeData > &data)
Calls all registered callbacks on the specified output port.
Definition Node.cpp:180
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:413
NAV::OutputPin::PollDataFunc
std::shared_ptr< const NAV::NodeData >(Node::*)() PollDataFunc
FileReader/Simulator pollData function type for nodes with a single poll pin.
Definition Pin.hpp:445
NAV::PosVelAttInitializer::_inputPinIdxGNSS
int _inputPinIdxGNSS
Index of the input pin for GNSS observations.
Definition PosVelAttInitializer.hpp:75
NAV::PosVelAttInitializer::_overrideRollPitchYawValues
std::array< double, 3 > _overrideRollPitchYawValues
Values to override Roll, Pitch and Yaw with in [deg].
Definition PosVelAttInitializer.hpp:186
NAV::PosVelAttInitializer::_overridePosition
bool _overridePosition
Whether the GNSS values should be used or we want to override the values manually.
Definition PosVelAttInitializer.hpp:143
NAV::PosVelAttInitializer::receivePosObs
void receivePosObs(const std::shared_ptr< const Pos > &obs)
Receive Pos Observations.
Definition PosVelAttInitializer.cpp:777
NAV::PosVelAttInitializer::deinitialize
void deinitialize() override
Deinitialize the node.
Definition PosVelAttInitializer.cpp:492
NAV::PosVelAttInitializer::_posVelAttInitialized
std::array< bool, 4 > _posVelAttInitialized
Whether the states are initialized (pos, vel, att, messages send)
Definition PosVelAttInitializer.hpp:189
NAV::PosVelAttInitializer::_initTimeEditFormat
gui::widgets::TimeEditFormat _initTimeEditFormat
Time Format to input the init time with.
Definition PosVelAttInitializer.hpp:192
NAV::PosVelAttInitializer::_averagedAttitude
std::array< double, 3 > _averagedAttitude
Averaged Attitude (roll, pitch, yaw) in [rad].
Definition PosVelAttInitializer.hpp:182
NAV::PosVelAttInitializer::_lastPositionAccuracy
std::array< double, 3 > _lastPositionAccuracy
Last position accuracy in [cm] for XYZ or NED.
Definition PosVelAttInitializer.hpp:150
NAV::PosVelAttInitializer::OUTPUT_PORT_INDEX_POS_VEL_ATT
static constexpr size_t OUTPUT_PORT_INDEX_POS_VEL_ATT
Flow (PosVelAtt)
Definition PosVelAttInitializer.hpp:70
NAV::PosVelAttInitializer::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition PosVelAttInitializer.cpp:52
NAV::PosVelAttInitializer::AttitudeMode::BOTH
@ BOTH
Use IMU and GNSS Observations for attitude initialization.
Definition PosVelAttInitializer.hpp:128
NAV::PosVelAttInitializer::AttitudeMode::IMU
@ IMU
Use IMU Observations for attitude initialization.
Definition PosVelAttInitializer.hpp:129
NAV::PosVelAttInitializer::AttitudeMode::GNSS
@ GNSS
Use GNSS Observations for attitude initialization.
Definition PosVelAttInitializer.hpp:130
NAV::PosVelAttInitializer::_overrideVelocityValues
Eigen::Vector3d _overrideVelocityValues
Values to override the Velocity in [m/s].
Definition PosVelAttInitializer.hpp:171
NAV::PosVelAttInitializer::receiveGnssObs
void receiveGnssObs(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Gnss Observations.
Definition PosVelAttInitializer.cpp:677
NAV::PosVelAttInitializer::_attitudeMode
AttitudeMode _attitudeMode
GUI option to pecify the initialization source for attitude.
Definition PosVelAttInitializer.hpp:140
NAV::PosVelAttInitializer::_e_initPosition
Eigen::Vector3d _e_initPosition
Position in ECEF coordinates.
Definition PosVelAttInitializer.hpp:199
NAV::PosVelAttInitializer::receivePosVelObs
void receivePosVelObs(const std::shared_ptr< const PosVel > &obs)
Receive PosVel Observations.
Definition PosVelAttInitializer.cpp:784
NAV::PosVelAttInitializer::type
std::string type() const override
String representation of the Class Type.
Definition PosVelAttInitializer.cpp:57
NAV::PosVelAttInitializer::_n_Quat_b_init
Eigen::Quaterniond _n_Quat_b_init
Initialized Quaternion body to navigation frame (roll, pitch, yaw)
Definition PosVelAttInitializer.hpp:197
NAV::PosVelAttInitializer::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition PosVelAttInitializer.cpp:67
NAV::PosVelAttInitializer::updatePins
void updatePins()
Add or removes input pins depending on the settings and modifies the output pin.
Definition PosVelAttInitializer.cpp:497
NAV::PosVelAttInitializer::_inputPinIdxIMU
int _inputPinIdxIMU
Index of the input pin for IMU observations.
Definition PosVelAttInitializer.hpp:73
NAV::PosVelAttInitializer::_velocityAccuracyThreshold
double _velocityAccuracyThreshold
Velocity Accuracy to achieve in [cm/s].
Definition PosVelAttInitializer.hpp:173
NAV::PosVelAttInitializer::_initDuration
double _initDuration
Time in [s] to initialize the state.
Definition PosVelAttInitializer.hpp:120
NAV::PosVelAttInitializer::restore
void restore(const json &j) override
Restores the node from a json object.
Definition PosVelAttInitializer.cpp:411
NAV::PosVelAttInitializer::~PosVelAttInitializer
~PosVelAttInitializer() override
Destructor.
Definition PosVelAttInitializer.cpp:47
NAV::PosVelAttInitializer::PosVelAttInitializer
PosVelAttInitializer()
Default constructor.
Definition PosVelAttInitializer.cpp:34
NAV::PosVelAttInitializer::initialize
bool initialize() override
Initialize the node.
Definition PosVelAttInitializer.cpp:466
NAV::PosVelAttInitializer::_n_initVelocity
Eigen::Vector3d _n_initVelocity
Velocity in navigation coordinates.
Definition PosVelAttInitializer.hpp:201
NAV::PosVelAttInitializer::_overrideRollPitchYaw
std::array< bool, 3 > _overrideRollPitchYaw
Whether the IMU values should be used or we want to override the values manually.
Definition PosVelAttInitializer.hpp:184
NAV::PosVelAttInitializer::pollPVASolution
std::shared_ptr< const NodeData > pollPVASolution()
Polls the PVA solution if all is set in the GUI.
Definition PosVelAttInitializer.cpp:818
NAV::PosVelAttInitializer::save
json save() const override
Saves the node into a json object.
Definition PosVelAttInitializer.cpp:389
NAV::PosVelAttInitializer::finalizeInit
void finalizeInit()
Checks whether all Flags are set and writes logs messages.
Definition PosVelAttInitializer.cpp:542
NAV::PosVelAttInitializer::_startTime
InsTime _startTime
Start time of the averaging process.
Definition PosVelAttInitializer.hpp:123
NAV::PosVelAttInitializer::_initTime
InsTime _initTime
Initialization time.
Definition PosVelAttInitializer.hpp:195
NAV::PosVelAttInitializer::_positionAccuracyThreshold
double _positionAccuracyThreshold
Position Accuracy to achieve in [cm].
Definition PosVelAttInitializer.hpp:148
NAV::PosVelAttInitializer::category
static std::string category()
String representation of the Class Category.
Definition PosVelAttInitializer.cpp:62
NAV::PosVelAttInitializer::receivePosVelAttObs
void receivePosVelAttObs(const std::shared_ptr< const PosVelAtt > &obs)
Receive PosVelAtt Observations.
Definition PosVelAttInitializer.cpp:793
NAV::PosVelAttInitializer::receiveImuObs
void receiveImuObs(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Imu Observations.
Definition PosVelAttInitializer.cpp:615
NAV::PosVelAttInitializer::_overridePositionValue
gui::widgets::PositionWithFrame _overridePositionValue
Values to override the Position in ECEF coordinates in [m].
Definition PosVelAttInitializer.hpp:145
NAV::PosVelAttInitializer::_countAveragedAttitude
double _countAveragedAttitude
Count of received attitude measurements.
Definition PosVelAttInitializer.hpp:180
NAV::PosVelAttInitializer::_overrideVelocity
VelocityOverride _overrideVelocity
Whether the GNSS values should be used or we want to override the values manually.
Definition PosVelAttInitializer.hpp:169
NAV::PosVelAttInitializer::_lastVelocityAccuracy
std::array< double, 3 > _lastVelocityAccuracy
Last velocity accuracy in [cm/s] for XYZ or NED.
Definition PosVelAttInitializer.hpp:175
NAV::PosVelAttInitializer::receiveUbloxObs
void receiveUbloxObs(const std::shared_ptr< const UbloxObs > &obs)
Receive Ublox Observations.
Definition PosVelAttInitializer.cpp:706
NAV::PosVelAtt::type
static std::string type()
Returns the type of the data class.
Definition PosVelAtt.hpp:29
NAV::PosVel::type
static std::string type()
Returns the type of the data class.
Definition PosVel.hpp:27
NAV::Pos::type
static std::string type()
Returns the type of the data class.
Definition Pos.hpp:36
NAV::RtklibPosObs::type
static std::string type()
Returns the type of the data class.
Definition RtklibPosObs.hpp:139
NAV::TsDeque::extract_front
auto extract_front()
Returns a copy of the first element in the container and removes it from the container.
Definition TsDeque.hpp:494
NAV::UbloxObs::type
static std::string type()
Returns the type of the data class.
Definition UbloxObs.hpp:30
NAV::gui::NodeEditorApplication::windowFontRatio
static float windowFontRatio()
Ratio to multiply for GUI window elements.
Definition NodeEditorApplication.cpp:1916
imgui_ex.hpp
ImGui extensions.
ImGui::DragDouble
bool DragDouble(const char *label, double *v, float v_speed, double v_min, double v_max, const char *format, ImGuiSliderFlags flags)
Shows a Drag GUI element for 'double'.
Definition imgui_ex.cpp:19
NAV::NodeManager::CreateOutputPin
OutputPin * CreateOutputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier, OutputPin::PinData data=static_cast< void * >(nullptr), int idx=-1)
Create an Output Pin object.
Definition NodeManager.cpp:202
NAV::NodeManager::DeleteInputPin
bool DeleteInputPin(InputPin &pin)
Deletes the input pin. Invalidates the pin reference given.
Definition NodeManager.cpp:234
NAV::NodeManager::CreateInputPin
InputPin * CreateInputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier={}, InputPin::Callback callback=static_cast< InputPin::FlowFirableCallbackFunc >(nullptr), InputPin::FlowFirableCheckFunc firable=nullptr, int priority=0, int idx=-1)
Create an Input Pin object.
Definition NodeManager.cpp:176
NAV::NodeRegistry::NodeDataTypeAnyIsChildOf
bool NodeDataTypeAnyIsChildOf(const std::vector< std::string > &childTypes, const std::vector< std::string > &parentTypes)
Checks if any of the provided child types is a child of any of the provided parent types.
Definition NodeRegistry.cpp:113
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:442
NAV::gui::widgets::TimeEdit
bool TimeEdit(const char *str_id, InsTime &insTime, TimeEditFormat &timeEditFormat, float itemWidth=170.0F)
Inputs to edit an InsTime object.
Definition TimeEdit.cpp:52
NAV::gui::widgets::EnumCombo
bool EnumCombo(const char *label, T &enumeration, size_t startIdx=0)
Combo representing an enumeration.
Definition EnumCombo.hpp:30
NAV::gui::widgets::PositionInput
bool PositionInput(const char *str, PositionWithFrame &position, PositionInputLayout layout=PositionInputLayout::SINGLE_COLUMN, float itemWidth=140.0F)
Inputs to edit an Position object.
Definition PositionInput.cpp:55
NAV::gui::widgets::PositionInputLayout::SINGLE_COLUMN
@ SINGLE_COLUMN
All elements in a single column.
Definition PositionInput.hpp:86
NAV::trafo::ecef2lla_WGS84
Eigen::Vector3< typename Derived::Scalar > ecef2lla_WGS84(const Eigen::MatrixBase< Derived > &e_position)
Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84.
Definition CoordinateFrames.hpp:420
NAV::trafo::n_Quat_e
Eigen::Quaternion< Scalar > n_Quat_e(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from Earth-fixed to navigation frame.
Definition CoordinateFrames.hpp:320
NAV::trafo::quat2eulerZYX
Eigen::Vector3< typename Derived::Scalar > quat2eulerZYX(const Eigen::QuaternionBase< Derived > &q)
Converts the quaternion to Euler rotation angles with rotation sequence ZYX.
Definition CoordinateFrames.hpp:141
NAV::trafo::lla2ecef_WGS84
Eigen::Vector3< typename Derived::Scalar > lla2ecef_WGS84(const Eigen::MatrixBase< Derived > &lla_position)
Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84.
Definition CoordinateFrames.hpp:402
NAV::trafo::n_Quat_b
Eigen::Quaternion< Scalar > n_Quat_b(Scalar roll, Scalar pitch, Scalar yaw)
Quaternion for rotations from body to navigation frame.
Definition CoordinateFrames.hpp:357
NAV::vendor::ublox::UBX_CLASS_NAV
@ UBX_CLASS_NAV
Navigation Results Messages: Position, Speed, Time, Acceleration, Heading, DOP, SVs used.
Definition UbloxTypes.hpp:92
NAV::vendor::ublox::UbxNavMessages
UbxNavMessages
The available NAV Messages.
Definition UbloxTypes.hpp:482
NAV::vendor::ublox::UBX_NAV_VELNED
@ UBX_NAV_VELNED
Velocity Solution in NED (Length = 36; Type = Periodic/Polled)
Definition UbloxTypes.hpp:544
NAV::vendor::ublox::UBX_NAV_ATT
@ UBX_NAV_ATT
Attitude Solution (Length = 32; Type = Periodic/Polled)
Definition UbloxTypes.hpp:486
NAV::vendor::ublox::UBX_NAV_POSECEF
@ UBX_NAV_POSECEF
Position Solution in ECEF (Length = 20; Type = Periodic/Polled)
Definition UbloxTypes.hpp:506
NAV::vendor::ublox::UBX_NAV_POSLLH
@ UBX_NAV_POSLLH
Geodetic Position Solution (Length = 28; Type = Periodic/Polled)
Definition UbloxTypes.hpp:508
NAV::to_string
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Definition PositionInput.cpp:25
NAV::calcRollFromStaticAcceleration
Derived::Scalar calcRollFromStaticAcceleration(const Eigen::MatrixBase< Derived > &b_accel)
Calculates the roll angle from a static acceleration measurement.
Definition Functions.hpp:118
NAV::deg2rad
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
NAV::calcPitchFromStaticAcceleration
Derived::Scalar calcPitchFromStaticAcceleration(const Eigen::MatrixBase< Derived > &b_accel)
Calculates the pitch angle from a static acceleration measurement.
Definition Functions.hpp:133
NAV::rad2deg
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39
NAV::InsTime_GPSweekTow
GPS week and time of week in GPS standard time [GPST].
Definition InsTime.hpp:369
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52