0.5.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/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
32NAV::PosVelAttInitializer::PosVelAttInitializer()
33 : Node(typeStatic())
34{
35 LOG_TRACE("{}: called", name);
36
37 _hasConfig = true;
38 _guiConfigDefaultWindowSize = { 345, 342 };
39
40 CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
41
42 updatePins();
43}
44
49
50std::string NAV::PosVelAttInitializer::typeStatic()
51{
52 return "PosVelAttInitializer";
53}
54
55std::string NAV::PosVelAttInitializer::type() const
56{
57 return typeStatic();
58}
59
60std::string NAV::PosVelAttInitializer::category()
61{
62 return "State";
63}
64
65void 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
409void NAV::PosVelAttInitializer::restore(json const& j)
410{
411 LOG_TRACE("{}: called", nameId());
412
413 if (j.contains("initDuration"))
414 {
415 j.at("initDuration").get_to(_initDuration);
416 }
417 if (j.contains("attitudeMode"))
418 {
419 j.at("attitudeMode").get_to(_attitudeMode);
420 }
421 if (j.contains("positionAccuracyThreshold"))
422 {
423 j.at("positionAccuracyThreshold").get_to(_positionAccuracyThreshold);
424 }
425 if (j.contains("velocityAccuracyThreshold"))
426 {
427 j.at("velocityAccuracyThreshold").get_to(_velocityAccuracyThreshold);
428 }
429 if (j.contains("overridePosition"))
430 {
431 j.at("overridePosition").get_to(_overridePosition);
432 }
433 if (j.contains("overridePositionValues"))
434 {
435 j.at("overridePositionValues").get_to(_overridePositionValue);
436 }
437 if (j.contains("overrideVelocity"))
438 {
439 j.at("overrideVelocity").get_to(_overrideVelocity);
440 }
441 if (j.contains("overrideVelocityValues"))
442 {
443 j.at("overrideVelocityValues").get_to(_overrideVelocityValues);
444 }
445 if (j.contains("overrideRollPitchYaw"))
446 {
447 j.at("overrideRollPitchYaw").get_to(_overrideRollPitchYaw);
448 }
449 if (j.contains("overrideRollPitchYawValues"))
450 {
451 j.at("overrideRollPitchYawValues").get_to(_overrideRollPitchYawValues);
452 }
453 if (j.contains("initTime"))
454 {
455 j.at("initTime").get_to(_initTime);
456 }
457 if (j.contains("initTimeEditFormat"))
458 {
459 j.at("initTimeEditFormat").get_to(_initTimeEditFormat);
460 }
461 updatePins();
462}
463
464bool NAV::PosVelAttInitializer::initialize()
465{
466 LOG_TRACE("{}: called", nameId());
467
468 _startTime.reset();
469
470 _countAveragedAttitude = 0.0;
471
472 _lastPositionAccuracy = { std::numeric_limits<double>::infinity(),
473 std::numeric_limits<double>::infinity(),
474 std::numeric_limits<double>::infinity() };
475 _lastVelocityAccuracy = { std::numeric_limits<double>::infinity(),
476 std::numeric_limits<double>::infinity(),
477 std::numeric_limits<double>::infinity() };
478
479 _posVelAttInitialized = { false, false, false, false };
480
481 if (!_overridePosition || _overrideVelocity == VelocityOverride::OFF
482 || !_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2])
483 {
484 _initTime = InsTime(InsTime_GPSweekTow(0, 0, 0));
485 }
486
487 return true;
488}
489
490void NAV::PosVelAttInitializer::deinitialize()
491{
492 LOG_TRACE("{}: called", nameId());
493}
494
495void NAV::PosVelAttInitializer::updatePins()
496{
497 if (_overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2] && _inputPinIdxIMU >= 0)
498 {
499 DeleteInputPin(static_cast<size_t>(_inputPinIdxIMU));
500 _inputPinIdxIMU = -1;
501 _inputPinIdxGNSS--;
502 }
503 else if ((!_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2]) && _inputPinIdxIMU < 0)
504 {
505 CreateInputPin("ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }, &PosVelAttInitializer::receiveImuObs, nullptr, 0, 0);
506 _inputPinIdxIMU = 0;
507 if (_inputPinIdxGNSS >= 0)
508 {
509 _inputPinIdxGNSS++;
510 }
511 }
512
513 if (_overridePosition
514 && _overrideVelocity != VelocityOverride::OFF
515 && _overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2] && _inputPinIdxGNSS >= 0)
516 {
517 DeleteInputPin(static_cast<size_t>(_inputPinIdxGNSS));
518 _inputPinIdxGNSS = -1;
519 }
520 else if ((!_overridePosition || _overrideVelocity == VelocityOverride::OFF
521 || !_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2])
522 && _inputPinIdxGNSS < 0)
523 {
524 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 _inputPinIdxGNSS = static_cast<int>(inputPins.size()) - 1;
528 }
529
530 if (_inputPinIdxGNSS < 0 && _inputPinIdxIMU < 0)
531 {
532 outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<OutputPin::PollDataFunc>(&PosVelAttInitializer::pollPVASolution);
533 }
534 else
535 {
536 outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<void*>(nullptr);
537 }
538}
539
540void 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
613void 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
675void 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
704void 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
775void 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
782void 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
791void 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
816std::shared_ptr<const NAV::NodeData> NAV::PosVelAttInitializer::pollPVASolution()
817{
818 if (_inputPinIdxIMU >= 0 || _inputPinIdxGNSS >= 0)
819 {
820 return nullptr;
821 }
822
823 int initCount = 0;
824 if (_overridePosition)
825 {
826 _e_initPosition = _overridePositionValue.e_position;
827 ++initCount;
828 }
829 if (_overrideVelocity == VelocityOverride::NED)
830 {
831 _n_initVelocity = _overrideVelocityValues;
832 ++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 if (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))
842 {
843 _n_Quat_b_init = trafo::n_Quat_b(deg2rad(_overrideRollPitchYawValues.at(0)),
844 deg2rad(_overrideRollPitchYawValues.at(1)),
845 deg2rad(_overrideRollPitchYawValues.at(2)));
846 ++initCount;
847 }
848 if (initCount == 3 && !_posVelAttInitialized.at(3))
849 {
850 auto posVelAtt = std::make_shared<PosVelAtt>();
851 posVelAtt->insTime = _initTime;
852
853 _posVelAttInitialized.at(3) = true;
854 posVelAtt->setPosition_e(_e_initPosition);
855 posVelAtt->setVelocity_n(_n_initVelocity);
856 posVelAtt->setAttitude_n_Quat_b(_n_Quat_b_init);
857
858 invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, posVelAtt);
859 return posVelAtt;
860 }
861 return nullptr;
862}
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:23
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
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:353
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:522
NAV::Node::outputPins
std::vector< OutputPin > outputPins
List of output pins.
Definition Node.hpp:511
NAV::Node::Node
Node(std::string name)
Constructor.
Definition Node.cpp:29
NAV::Node::inputPins
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:509
NAV::Node::CreateOutputPin
OutputPin * CreateOutputPin(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 Node.cpp:278
NAV::Node::nameId
std::string nameId() const
Node name and id.
Definition Node.cpp:323
NAV::Node::name
std::string name
Name of the Node.
Definition Node.hpp:507
NAV::Node::CreateInputPin
InputPin * CreateInputPin(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 Node.cpp:252
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:179
NAV::Node::DeleteInputPin
bool DeleteInputPin(size_t pinIndex)
Deletes the input pin. Invalidates the pin reference given.
Definition Node.cpp:310
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:525
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:775
NAV::PosVelAttInitializer::deinitialize
void deinitialize() override
Deinitialize the node.
Definition PosVelAttInitializer.cpp:490
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:50
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:675
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:782
NAV::PosVelAttInitializer::type
std::string type() const override
String representation of the Class Type.
Definition PosVelAttInitializer.cpp:55
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:65
NAV::PosVelAttInitializer::updatePins
void updatePins()
Add or removes input pins depending on the settings and modifies the output pin.
Definition PosVelAttInitializer.cpp:495
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:409
NAV::PosVelAttInitializer::~PosVelAttInitializer
~PosVelAttInitializer() override
Destructor.
Definition PosVelAttInitializer.cpp:45
NAV::PosVelAttInitializer::PosVelAttInitializer
PosVelAttInitializer()
Default constructor.
Definition PosVelAttInitializer.cpp:32
NAV::PosVelAttInitializer::initialize
bool initialize() override
Initialize the node.
Definition PosVelAttInitializer.cpp:464
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:816
NAV::PosVelAttInitializer::save
json save() const override
Saves the node into a json object.
Definition PosVelAttInitializer.cpp:387
NAV::PosVelAttInitializer::finalizeInit
void finalizeInit()
Checks whether all Flags are set and writes logs messages.
Definition PosVelAttInitializer.cpp:540
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:60
NAV::PosVelAttInitializer::receivePosVelAttObs
void receivePosVelAttObs(const std::shared_ptr< const PosVelAtt > &obs)
Receive PosVelAtt Observations.
Definition PosVelAttInitializer.cpp:791
NAV::PosVelAttInitializer::receiveImuObs
void receiveImuObs(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Imu Observations.
Definition PosVelAttInitializer.cpp:613
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:704
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:1922
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::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:682
NAV::gui::widgets::TimeEdit
bool TimeEdit(const char *str_id, InsTime &insTime, TimeEditFormat &timeEditFormat, float itemWidth=170.0F, int columns=1)
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:424
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:324
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:406
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:361
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