0.5.1
Loading...
Searching...
No Matches
ImuSimulator.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 "ImuSimulator.hpp"
10
11#include <cmath>
12#include <ctime>
13#include <optional>
14
15#include "Navigation/Time/InsTime.hpp"
16#include "util/Logger.hpp"
17#include "Navigation/Ellipsoid/Ellipsoid.hpp"
18#include "Navigation/INS/Functions.hpp"
19#include "Navigation/INS/LocalNavFrame/Mechanization.hpp"
20#include "Navigation/Gravity/Gravity.hpp"
21#include "Navigation/Math/Math.hpp"
22#include "Navigation/Transformations/Units.hpp"
23
24#include <Eigen/src/Geometry/AngleAxis.h>
25#include <Eigen/src/Geometry/Quaternion.h>
26#include "internal/FlowManager.hpp"
27#include "internal/gui/widgets/imgui_ex.hpp"
28#include "internal/gui/widgets/HelpMarker.hpp"
29#include "internal/gui/NodeEditorApplication.hpp"
30
31#include "NodeData/IMU/ImuObsSimulated.hpp"
32#include "NodeData/State/PosVelAtt.hpp"
33
45
46NAV::ImuSimulator::~ImuSimulator()
47{
48 LOG_TRACE("{}: called", nameId());
49}
50
51std::string NAV::ImuSimulator::typeStatic()
52{
53 return "ImuSimulator";
54}
55
56std::string NAV::ImuSimulator::type() const
57{
58 return typeStatic();
59}
60
61std::string NAV::ImuSimulator::category()
62{
63 return "Data Simulator";
64}
65
66void NAV::ImuSimulator::guiConfig()
67{
68 float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio();
69
70 if (_trajectoryType != TrajectoryType::Csv && ImGui::TreeNode("Start Time"))
71 {
72 auto startTimeSource = static_cast<int>(_startTimeSource);
73 if (ImGui::RadioButton(fmt::format("Current Computer Time##{}", size_t(id)).c_str(), &startTimeSource, static_cast<int>(StartTimeSource::CurrentComputerTime)))
74 {
75 _startTimeSource = static_cast<decltype(_startTimeSource)>(startTimeSource);
76 LOG_DEBUG("{}: startTimeSource changed to {}", nameId(), fmt::underlying(_startTimeSource));
77 flow::ApplyChanges();
78 }
79 if (_startTimeSource == StartTimeSource::CurrentComputerTime)
80 {
81 ImGui::Indent();
82
83 std::time_t t = std::time(nullptr);
84 std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe)
85
86 ImGui::Text("%d-%02d-%02d %02d:%02d:%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday, now->tm_hour, now->tm_min, now->tm_sec);
87
88 ImGui::Unindent();
89 }
90
91 if (ImGui::RadioButton(fmt::format("Custom Time##{}", size_t(id)).c_str(), &startTimeSource, static_cast<int>(StartTimeSource::CustomTime)))
92 {
93 _startTimeSource = static_cast<decltype(_startTimeSource)>(startTimeSource);
94 LOG_DEBUG("{}: startTimeSource changed to {}", nameId(), fmt::underlying(_startTimeSource));
95 flow::ApplyChanges();
96 }
97 if (_startTimeSource == StartTimeSource::CustomTime)
98 {
99 ImGui::Indent();
100 if (gui::widgets::TimeEdit(fmt::format("{}", size_t(id)).c_str(), _startTime, _startTimeEditFormat))
101 {
102 LOG_DEBUG("{}: startTime changed to {}", nameId(), _startTime);
103 flow::ApplyChanges();
104 }
105 ImGui::Unindent();
106 }
107 ImGui::TreePop();
108 }
109
110 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
111 if (ImGui::TreeNode("Output data-rate"))
112 {
113 ImGui::SetNextItemWidth(columnWidth);
114 if (ImGui::InputDoubleL(fmt::format("IMU internal rate##{}", size_t(id)).c_str(), &_imuInternalFrequency, 1e-3, 1e4, 0.0, 0.0, "%.3f Hz"))
115 {
116 LOG_DEBUG("{}: imuInternalFrequency changed to {}", nameId(), _imuInternalFrequency);
117 flow::ApplyChanges();
118 }
119 ImGui::SameLine();
120 gui::widgets::HelpMarker("Used to calculate the dTime, dTheta, dVel fields");
121 ImGui::SetNextItemWidth(columnWidth);
122 if (ImGui::InputDoubleL(fmt::format("IMU output rate##{}", size_t(id)).c_str(), &_imuFrequency, 1e-3, _imuInternalFrequency, 0.0, 0.0, "%.3f Hz"))
123 {
124 LOG_DEBUG("{}: imuFrequency changed to {}", nameId(), _imuFrequency);
125 flow::ApplyChanges();
126 }
127 ImGui::SetNextItemWidth(columnWidth);
128 if (ImGui::InputDouble(fmt::format("Trajectory output rate##{}", size_t(id)).c_str(), &_gnssFrequency, 0.0, 0.0, "%.3f Hz"))
129 {
130 LOG_DEBUG("{}: gnssFrequency changed to {}", nameId(), _gnssFrequency);
131 flow::ApplyChanges();
132 }
133
134 ImGui::TreePop();
135 }
136
137 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
138 if (ImGui::TreeNode("Scenario"))
139 {
140 ImGui::SetNextItemWidth(columnWidth);
141 if (ImGui::BeginCombo(fmt::format("Trajectory##{}", size_t(id)).c_str(), to_string(_trajectoryType)))
142 {
143 for (size_t i = 0; i < static_cast<size_t>(TrajectoryType::COUNT); i++)
144 {
145 const bool is_selected = (static_cast<size_t>(_trajectoryType) == i);
146 if (ImGui::Selectable(to_string(static_cast<TrajectoryType>(i)), is_selected))
147 {
148 _trajectoryType = static_cast<TrajectoryType>(i);
149 LOG_DEBUG("{}: trajectoryType changed to {}", nameId(), fmt::underlying(_trajectoryType));
150
151 if (_trajectoryType == TrajectoryType::Csv && inputPins.empty())
152 {
153 CreateInputPin(CsvData::type().c_str(), Pin::Type::Object, { CsvData::type() });
154 }
155 else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty())
156 {
157 DeleteInputPin(0);
158 }
159
160 flow::ApplyChanges();
161 doDeinitialize();
162 }
163
164 if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
165 {
166 ImGui::SetItemDefaultFocus();
167 }
168 }
169 ImGui::EndCombo();
170 }
171 if (_trajectoryType == TrajectoryType::Csv)
172 {
173 auto TextColoredIfExists = [this](int index, const char* text, const char* type) {
174 ImGui::TableSetColumnIndex(index);
175 auto displayTable = [&]() {
176 if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV);
177 csvData && std::ranges::find(csvData->v->description, text) != csvData->v->description.end())
178 {
179 ImGui::TextUnformatted(text);
180 ImGui::TableNextColumn();
181 ImGui::TextUnformatted(type);
182 }
183 else
184 {
185 ImGui::TextDisabled("%s", text);
186 ImGui::TableNextColumn();
187 ImGui::TextDisabled("%s", type);
188 }
189 };
190 displayTable();
191 };
192
193 if (ImGui::TreeNode(fmt::format("Format description##{}", size_t(id)).c_str()))
194 {
195 ImGui::TextUnformatted("Time information:");
196 ImGui::SameLine();
197 gui::widgets::HelpMarker("You can either provide the time in GPS time or in UTC time.");
198 if (ImGui::BeginTable(fmt::format("##Time ({})", size_t(id)).c_str(), 5,
199 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX))
200 {
201 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
202 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
203 ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed);
204 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
205 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
206 ImGui::TableHeadersRow();
207
208 ImGui::TableNextRow();
209 TextColoredIfExists(0, "GpsCycle", "uint");
210 TextColoredIfExists(3, "YearUTC", "uint");
211 ImGui::TableNextRow();
212 TextColoredIfExists(0, "GpsWeek", "uint");
213 TextColoredIfExists(3, "MonthUTC", "uint");
214 ImGui::TableNextRow();
215 TextColoredIfExists(0, "GpsToW [s]", "uint");
216 TextColoredIfExists(3, "DayUTC", "uint");
217 ImGui::TableNextRow();
218 TextColoredIfExists(3, "HourUTC", "uint");
219 ImGui::TableNextRow();
220 TextColoredIfExists(3, "MinUTC", "uint");
221 ImGui::TableNextRow();
222 TextColoredIfExists(3, "SecUTC", "double");
223
224 ImGui::EndTable();
225 }
226
227 ImGui::TextUnformatted("Position information:");
228 ImGui::SameLine();
229 gui::widgets::HelpMarker("You can either provide the position in ECEF coordinates\nor in latitude, longitude and altitude.");
230 if (ImGui::BeginTable(fmt::format("##Position ({})", size_t(id)).c_str(), 5,
231 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX))
232 {
233 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
234 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
235 ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed);
236 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
237 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
238 ImGui::TableHeadersRow();
239
240 ImGui::TableNextRow();
241 TextColoredIfExists(0, "X-ECEF [m]", "double");
242 TextColoredIfExists(3, "Latitude [deg]", "double");
243 ImGui::TableNextRow();
244 TextColoredIfExists(0, "Y-ECEF [m]", "double");
245 TextColoredIfExists(3, "Longitude [deg]", "double");
246 ImGui::TableNextRow();
247 TextColoredIfExists(0, "Z-ECEF [m]", "double");
248 TextColoredIfExists(3, "Altitude [m]", "double");
249
250 ImGui::EndTable();
251 }
252
253 ImGui::TextUnformatted("Attitude information:");
254 ImGui::SameLine();
255 gui::widgets::HelpMarker("You can either provide the attitude as an angle or quaternion.");
256 if (ImGui::BeginTable(fmt::format("##Attitude ({})", size_t(id)).c_str(), 5,
257 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX))
258 {
259 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
260 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
261 ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed);
262 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
263 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
264 ImGui::TableHeadersRow();
265
266 ImGui::TableNextRow();
267 TextColoredIfExists(0, "Roll [deg]", "double");
268 TextColoredIfExists(3, "n_Quat_b w", "double");
269 ImGui::TableNextRow();
270 TextColoredIfExists(0, "Pitch [deg]", "double");
271 TextColoredIfExists(3, "n_Quat_b x", "double");
272 ImGui::TableNextRow();
273 TextColoredIfExists(0, "Yaw [deg]", "double");
274 TextColoredIfExists(3, "n_Quat_b y", "double");
275 ImGui::TableNextRow();
276 TextColoredIfExists(3, "n_Quat_b z", "double");
277
278 ImGui::EndTable();
279 }
280
281 ImGui::TreePop();
282 }
283 }
284 else
285 {
286 const auto* txt = _trajectoryType == TrajectoryType::Fixed
287 ? "Position"
288 : (_trajectoryType == TrajectoryType::Linear
289 ? "Start position"
290 : (_trajectoryType == TrajectoryType::Circular
291 ? "Center position"
292 : (_trajectoryType == TrajectoryType::RoseFigure
293 ? "Center/Tangential position"
294 : "")));
295
296 if (gui::widgets::PositionInput(fmt::format("{}##PosInput {}", txt, size_t(id)).c_str(), _startPosition, gui::widgets::PositionInputLayout::TWO_ROWS, columnWidth))
297 {
298 flow::ApplyChanges();
299 doDeinitialize();
300 }
301 }
302
303 if (_trajectoryType == TrajectoryType::Fixed)
304 {
305 ImGui::SetNextItemWidth(columnWidth);
306 double roll = rad2deg(_fixedTrajectoryStartOrientation.x());
307 if (ImGui::InputDoubleL(fmt::format("##Roll{}", size_t(id)).c_str(), &roll, -180, 180, 0.0, 0.0, "%.3f°"))
308 {
309 _fixedTrajectoryStartOrientation.x() = deg2rad(roll);
310 LOG_DEBUG("{}: roll changed to {}", nameId(), roll);
311 flow::ApplyChanges();
312 doDeinitialize();
313 }
314 ImGui::SameLine();
315 ImGui::SetNextItemWidth(columnWidth);
316 double pitch = rad2deg(_fixedTrajectoryStartOrientation.y());
317 if (ImGui::InputDoubleL(fmt::format("##Pitch{}", size_t(id)).c_str(), &pitch, -90, 90, 0.0, 0.0, "%.3f°"))
318 {
319 _fixedTrajectoryStartOrientation.y() = deg2rad(pitch);
320 LOG_DEBUG("{}: pitch changed to {}", nameId(), pitch);
321 flow::ApplyChanges();
322 doDeinitialize();
323 }
324 ImGui::SameLine();
325 ImGui::SetNextItemWidth(columnWidth);
326 double yaw = rad2deg(_fixedTrajectoryStartOrientation.z());
327 if (ImGui::InputDoubleL(fmt::format("##Yaw{}", size_t(id)).c_str(), &yaw, -180, 180, 0.0, 0.0, "%.3f°"))
328 {
329 _fixedTrajectoryStartOrientation.z() = deg2rad(yaw);
330 LOG_DEBUG("{}: yaw changed to {}", nameId(), yaw);
331 flow::ApplyChanges();
332 doDeinitialize();
333 }
334 ImGui::SameLine();
335 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
336 ImGui::TextUnformatted("Orientation (Roll, Pitch, Yaw)");
337 }
338 else if (_trajectoryType == TrajectoryType::Linear)
339 {
340 ImGui::SetNextItemWidth(columnWidth);
341 if (ImGui::InputDouble(fmt::format("##North velocity{}", size_t(id)).c_str(), &_n_linearTrajectoryStartVelocity.x(), 0.0, 0.0, "%.3f m/s"))
342 {
343 LOG_DEBUG("{}: n_linearTrajectoryStartVelocity changed to {}", nameId(), _n_linearTrajectoryStartVelocity.transpose());
344 flow::ApplyChanges();
345 doDeinitialize();
346 }
347 ImGui::SameLine();
348 ImGui::SetNextItemWidth(columnWidth);
349 if (ImGui::InputDouble(fmt::format("##East velocity{}", size_t(id)).c_str(), &_n_linearTrajectoryStartVelocity.y(), 0.0, 0.0, "%.3f m/s"))
350 {
351 LOG_DEBUG("{}: n_linearTrajectoryStartVelocity changed to {}", nameId(), _n_linearTrajectoryStartVelocity.transpose());
352 flow::ApplyChanges();
353 doDeinitialize();
354 }
355 ImGui::SameLine();
356 ImGui::SetNextItemWidth(columnWidth);
357 if (ImGui::InputDouble(fmt::format("##Down velocity{}", size_t(id)).c_str(), &_n_linearTrajectoryStartVelocity.z(), 0.0, 0.0, "%.3f m/s"))
358 {
359 LOG_DEBUG("{}: n_linearTrajectoryStartVelocity changed to {}", nameId(), _n_linearTrajectoryStartVelocity.transpose());
360 flow::ApplyChanges();
361 doDeinitialize();
362 }
363 ImGui::SameLine();
364 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
365 ImGui::TextUnformatted("Velocity (North, East, Down)");
366 }
367 else if (_trajectoryType == TrajectoryType::Circular || _trajectoryType == TrajectoryType::RoseFigure)
368 {
369 if (ImGui::BeginTable(fmt::format("CircularOrRoseTrajectory##{}", size_t(id)).c_str(), 2, ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX))
370 {
371 ImGui::TableNextColumn();
372 ImGui::SetNextItemWidth(columnWidth);
373 if (ImGui::InputDoubleL(fmt::format("{}##{}", _trajectoryType == TrajectoryType::Circular ? "Radius" : "Radius/Amplitude", size_t(id)).c_str(), &_trajectoryRadius, 1e-3, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3f m"))
374 {
375 LOG_DEBUG("{}: circularTrajectoryRadius changed to {}", nameId(), _trajectoryRadius);
376 flow::ApplyChanges();
377 doDeinitialize();
378 }
379
380 ImGui::TableNextColumn();
381 ImGui::SetNextItemWidth(columnWidth);
382 if (ImGui::InputDouble(fmt::format("Horizontal speed##{}", size_t(id)).c_str(), &_trajectoryHorizontalSpeed, 0.0, 0.0, "%.3f m/s"))
383 {
384 LOG_DEBUG("{}: circularTrajectoryHorizontalSpeed changed to {}", nameId(), _trajectoryHorizontalSpeed);
385 flow::ApplyChanges();
386 doDeinitialize();
387 }
388 // ####################################################################################################
389 ImGui::TableNextColumn();
390 ImGui::SetNextItemWidth(columnWidth);
391 double originAngle = rad2deg(_trajectoryRotationAngle);
392 if (ImGui::DragDouble(fmt::format("{}##{}", _trajectoryType == TrajectoryType::Circular ? "Origin angle" : "Rotation angle", size_t(id)).c_str(), &originAngle, 15.0, -360.0, 360.0, "%.3f°"))
393 {
394 _trajectoryRotationAngle = deg2rad(originAngle);
395 LOG_DEBUG("{}: originAngle changed to {}", nameId(), originAngle);
396 flow::ApplyChanges();
397 doDeinitialize();
398 }
399
400 ImGui::TableNextColumn();
401 ImGui::SetNextItemWidth(columnWidth);
402 if (ImGui::InputDouble(fmt::format("Vertical speed (Up)##{}", size_t(id)).c_str(), &_trajectoryVerticalSpeed, 0.0, 0.0, "%.3f m/s"))
403 {
404 LOG_DEBUG("{}: circularTrajectoryVerticalSpeed changed to {}", nameId(), _trajectoryVerticalSpeed);
405 flow::ApplyChanges();
406 doDeinitialize();
407 }
408 // ####################################################################################################
409 ImGui::TableNextColumn();
410 auto tableStartX = ImGui::GetCursorPosX();
411 ImGui::SetNextItemWidth(200 * gui::NodeEditorApplication::windowFontRatio());
412 if (ImGui::BeginCombo(fmt::format("Motion##{}", size_t(id)).c_str(), to_string(_trajectoryDirection)))
413 {
414 for (size_t i = 0; i < static_cast<size_t>(Direction::COUNT); i++)
415 {
416 const bool is_selected = (static_cast<size_t>(_trajectoryDirection) == i);
417 if (ImGui::Selectable(to_string(static_cast<Direction>(i)), is_selected))
418 {
419 _trajectoryDirection = static_cast<Direction>(i);
420 LOG_DEBUG("{}: circularTrajectoryDirection changed to {}", nameId(), fmt::underlying(_trajectoryDirection));
421 flow::ApplyChanges();
422 doDeinitialize();
423 }
424
425 if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
426 {
427 ImGui::SetItemDefaultFocus();
428 }
429 }
430 ImGui::EndCombo();
431 }
432 ImGui::SetCursorPosX(tableStartX + columnWidth * 2.0F + ImGui::GetStyle().ItemSpacing.x * 1.0F);
433
434 ImGui::TableNextColumn();
435 // ####################################################################################################
436 if (_trajectoryType == TrajectoryType::Circular)
437 {
438 ImGui::TableNextColumn();
439 ImGui::SetNextItemWidth(columnWidth);
440 if (ImGui::DragInt(fmt::format("Osc Frequency##{}", size_t(id)).c_str(), &_circularHarmonicFrequency, 1.0F, 0, 100, "%d [cycles/rev]"))
441 {
442 LOG_DEBUG("{}: circularHarmonicFrequency changed to {}", nameId(), _circularHarmonicFrequency);
443 flow::ApplyChanges();
444 doDeinitialize();
445 }
446 ImGui::SameLine();
447 gui::widgets::HelpMarker("This modulates a harmonic oscillation on the circular path.\n"
448 "The frequency is in units [cycles per revolution].");
449
450 ImGui::TableNextColumn();
451 ImGui::SetNextItemWidth(columnWidth);
452 if (ImGui::DragDouble(fmt::format("Osc Amplitude Factor##{}", size_t(id)).c_str(), &_circularHarmonicAmplitudeFactor, 0.01F, 0.0, 10.0, "%.3f * r"))
453 {
454 LOG_DEBUG("{}: circularHarmonicAmplitudeFactor changed to {}", nameId(), _circularHarmonicAmplitudeFactor);
455 flow::ApplyChanges();
456 doDeinitialize();
457 }
458 ImGui::SameLine();
459 gui::widgets::HelpMarker("This modulates a harmonic oscillation on the circular path.\n"
460 "This factor determines the amplitude of the oscillation\n"
461 "with respect to the radius of the circle.");
462 }
463 else if (_trajectoryType == TrajectoryType::RoseFigure)
464 {
465 ImGui::TableNextColumn();
466 ImGui::SetNextItemWidth(columnWidth);
467 if (ImGui::InputIntL(fmt::format("n (angular freq.)##{}", size_t(id)).c_str(), &_rosePetNum, 1.0, std::numeric_limits<int>::max(), 1, 1))
468 {
469 LOG_DEBUG("{}: Rose figure numerator changed to {}", nameId(), _rosePetNum);
470 flow::ApplyChanges();
471 doDeinitialize();
472 }
473
474 ImGui::TableNextColumn();
475 ImGui::SetNextItemWidth(columnWidth);
476 if (ImGui::InputIntL(fmt::format("d (angular freq.)##{}", size_t(id)).c_str(), &_rosePetDenom, 1.0, std::numeric_limits<int>::max(), 1, 1))
477 {
478 LOG_DEBUG("{}: Rose figure denominator changed to {}", nameId(), _rosePetDenom);
479 flow::ApplyChanges();
480 doDeinitialize();
481 }
482 ImGui::SameLine();
483 if (gui::widgets::BeginHelpMarker())
484 {
485 ImGui::TextUnformatted("Angular frequency k=n/d, n,d = natural numbers. In case of d=1,\n"
486 "for even k number of petals is 2*k, for odd k, number of petals is k.\n"
487 "Adjusts the integration limits of the incomplete elliptical integral\n"
488 "of the second kind. If k is rational (d >1), the correct limit needs \n"
489 "to be choosen depending on the symmetry (petals) of the figure.");
490 float width = 500.0F;
491 ImGui::Image(gui::NodeEditorApplication::m_RoseFigure, ImVec2(width, width * 909.0F / 706.0F));
492 ImGui::TextUnformatted("Graphic by Jason Davies, CC BY-SA 3.0,\nhttps://commons.wikimedia.org/w/index.php?curid=30515231");
493 gui::widgets::EndHelpMarker();
494 }
495 }
496 // ####################################################################################################
497
498 ImGui::EndTable();
499 }
500 }
501 ImGui::TreePop();
502 }
503
504 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
505 if (_trajectoryType != TrajectoryType::Csv && ImGui::TreeNode("Simulation Stop Condition"))
506 {
507 auto simulationStopCondition = static_cast<int>(_simulationStopCondition);
508 if (_trajectoryType != TrajectoryType::Fixed)
509 {
510 if (ImGui::RadioButton(fmt::format("##simulationStopConditionDuration{}", size_t(id)).c_str(), &simulationStopCondition, static_cast<int>(StopCondition::Duration)))
511 {
512 _simulationStopCondition = static_cast<decltype(_simulationStopCondition)>(simulationStopCondition);
513 LOG_DEBUG("{}: simulationStopCondition changed to {}", nameId(), fmt::underlying(_simulationStopCondition));
514 flow::ApplyChanges();
515 doDeinitialize();
516 }
517 ImGui::SameLine();
518 }
519
520 ImGui::SetNextItemWidth(columnWidth);
521 if (ImGui::InputDoubleL(fmt::format("Duration##{}", size_t(id)).c_str(), &_simulationDuration, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3f s"))
522 {
523 LOG_DEBUG("{}: simulationDuration changed to {}", nameId(), _simulationDuration);
524 flow::ApplyChanges();
525 doDeinitialize();
526 }
527 if (_trajectoryType != TrajectoryType::Fixed)
528 {
529 if (ImGui::RadioButton(fmt::format("##simulationStopConditionDistanceOrCirclesOrRoses{}", size_t(id)).c_str(), &simulationStopCondition, static_cast<int>(StopCondition::DistanceOrCirclesOrRoses)))
530 {
531 _simulationStopCondition = static_cast<decltype(_simulationStopCondition)>(simulationStopCondition);
532 LOG_DEBUG("{}: simulationStopCondition changed to {}", nameId(), fmt::underlying(_simulationStopCondition));
533 flow::ApplyChanges();
534 doDeinitialize();
535 }
536 ImGui::SameLine();
537 }
538
539 if (_trajectoryType == TrajectoryType::Linear)
540 {
541 ImGui::SetNextItemWidth(columnWidth);
542 if (ImGui::InputDoubleL(fmt::format("Distance to start##{}", size_t(id)).c_str(), &_linearTrajectoryDistanceForStop, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3f m"))
543 {
544 LOG_DEBUG("{}: linearTrajectoryDistanceForStop changed to {}", nameId(), _linearTrajectoryDistanceForStop);
545 flow::ApplyChanges();
546 doDeinitialize();
547 }
548 }
549 else if (_trajectoryType == TrajectoryType::Circular)
550 {
551 ImGui::SetNextItemWidth(columnWidth);
552 if (ImGui::InputDoubleL(fmt::format("Number of Circles##{}", size_t(id)).c_str(), &_circularTrajectoryCircleCountForStop, 0.0, std::numeric_limits<double>::max(), 1.0, 1.0, "%.3f"))
553 {
554 LOG_DEBUG("{}: circularTrajectoryCircleCountForStop changed to {}", nameId(), _circularTrajectoryCircleCountForStop);
555 flow::ApplyChanges();
556 doDeinitialize();
557 }
558 }
559 else if (_trajectoryType == TrajectoryType::RoseFigure)
560 {
561 ImGui::SetNextItemWidth(columnWidth);
562 if (ImGui::InputDoubleL(fmt::format("Number of rose figures##{}", size_t(id)).c_str(), &_roseTrajectoryCountForStop, 0.0, std::numeric_limits<double>::max(), 1.0, 1.0, "%.3f"))
563 {
564 LOG_DEBUG("{}: RoseTrajectoryCountForStop changed to {}", nameId(), _roseTrajectoryCountForStop);
565 flow::ApplyChanges();
566 doDeinitialize();
567 }
568 }
569 ImGui::TreePop();
570 }
571
572 ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
573 if (ImGui::TreeNode("Simulation models"))
574 {
575 if (_trajectoryType != TrajectoryType::Fixed && _trajectoryType != TrajectoryType::Csv && _trajectoryType != TrajectoryType::RoseFigure)
576 {
577 ImGui::TextUnformatted(fmt::format("Spline (current knots {})", _splines.x.size()).c_str());
578 {
579 ImGui::Indent();
580 ImGui::SetNextItemWidth(columnWidth - ImGui::GetStyle().IndentSpacing);
581 if (ImGui::InputDoubleL(fmt::format("Sample Interval##{}", size_t(id)).c_str(), &_splines.sampleInterval, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3e s"))
582 {
583 LOG_DEBUG("{}: spline sample interval changed to {}", nameId(), _splines.sampleInterval);
584 flow::ApplyChanges();
585 doDeinitialize();
586 }
587 ImGui::Unindent();
588 }
589 }
590 else if (_trajectoryType == TrajectoryType::RoseFigure)
591 {
592 ImGui::TextUnformatted(fmt::format("Spline (current knots {})", _splines.x.size()).c_str());
593 {
594 ImGui::Indent();
595 ImGui::SetNextItemWidth(columnWidth - ImGui::GetStyle().IndentSpacing);
596 if (ImGui::InputDoubleL(fmt::format("Sample Distance##{}", size_t(id)).c_str(), &_roseStepLengthMax, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3e m"))
597 {
598 LOG_DEBUG("{}: Spline sample distance (rose figure) changed to {}", nameId(), _roseStepLengthMax);
599 flow::ApplyChanges();
600 doDeinitialize();
601 }
602 ImGui::Unindent();
603 }
604 }
605 ImGui::TextUnformatted("Measured acceleration");
606 {
607 ImGui::Indent();
608 ImGui::SetNextItemWidth(columnWidth - ImGui::GetStyle().IndentSpacing);
609
610 if (ComboGravitationModel(fmt::format("Gravitation Model##{}", size_t(id)).c_str(), _gravitationModel))
611 {
612 LOG_DEBUG("{}: Gravitation Model changed to {}", nameId(), NAV::to_string(_gravitationModel));
613 flow::ApplyChanges();
614 }
615 if (ImGui::Checkbox(fmt::format("Coriolis acceleration##{}", size_t(id)).c_str(), &_coriolisAccelerationEnabled))
616 {
617 LOG_DEBUG("{}: coriolisAccelerationEnabled changed to {}", nameId(), _coriolisAccelerationEnabled);
618 flow::ApplyChanges();
619 }
620 if (ImGui::Checkbox(fmt::format("Centrifugal acceleration##{}", size_t(id)).c_str(), &_centrifgalAccelerationEnabled))
621 {
622 LOG_DEBUG("{}: centrifgalAccelerationEnabled changed to {}", nameId(), _centrifgalAccelerationEnabled);
623 flow::ApplyChanges();
624 }
625 ImGui::Unindent();
626 }
627 ImGui::TextUnformatted("Measured angular rates");
628 {
629 ImGui::Indent();
630 if (ImGui::Checkbox(fmt::format("Earth rotation rate##{}", size_t(id)).c_str(), &_angularRateEarthRotationEnabled))
631 {
632 LOG_DEBUG("{}: angularRateEarthRotationEnabled changed to {}", nameId(), _angularRateEarthRotationEnabled);
633 flow::ApplyChanges();
634 }
635 if (ImGui::Checkbox(fmt::format("Transport rate##{}", size_t(id)).c_str(), &_angularRateTransportRateEnabled))
636 {
637 LOG_DEBUG("{}: angularRateTransportRateEnabled changed to {}", nameId(), _angularRateTransportRateEnabled);
638 flow::ApplyChanges();
639 }
640 ImGui::Unindent();
641 }
642
643 ImGui::TreePop();
644 }
645
646 Imu::guiConfig();
647}
648
649json NAV::ImuSimulator::save() const
650{
651 LOG_TRACE("{}: called", nameId());
652
653 json j;
654
655 j["startTimeSource"] = _startTimeSource;
656 j["startTimeEditFormat"] = _startTimeEditFormat;
657 j["startTime"] = _startTime;
658 // ###########################################################################################################
659 j["imuInternalFrequency"] = _imuInternalFrequency;
660 j["imuFrequency"] = _imuFrequency;
661 j["gnssFrequency"] = _gnssFrequency;
662 // ###########################################################################################################
663 j["trajectoryType"] = _trajectoryType;
664 j["startPosition"] = _startPosition;
665 j["fixedTrajectoryStartOrientation"] = _fixedTrajectoryStartOrientation;
666 j["n_linearTrajectoryStartVelocity"] = _n_linearTrajectoryStartVelocity;
667 j["circularHarmonicFrequency"] = _circularHarmonicFrequency;
668 j["circularHarmonicAmplitudeFactor"] = _circularHarmonicAmplitudeFactor;
669 j["trajectoryHorizontalSpeed"] = _trajectoryHorizontalSpeed;
670 j["trajectoryVerticalSpeed"] = _trajectoryVerticalSpeed;
671 j["trajectoryRadius"] = _trajectoryRadius;
672 j["trajectoryRotationAngle"] = _trajectoryRotationAngle;
673 j["trajectoryDirection"] = _trajectoryDirection;
674 j["rosePetNum"] = _rosePetNum;
675 j["rosePetDenom"] = _rosePetDenom;
676 // ###########################################################################################################
677 j["simulationStopCondition"] = _simulationStopCondition;
678 j["simulationDuration"] = _simulationDuration;
679 j["linearTrajectoryDistanceForStop"] = _linearTrajectoryDistanceForStop;
680 j["circularTrajectoryCircleCountForStop"] = _circularTrajectoryCircleCountForStop;
681 j["roseTrajectoryCountForStop"] = _roseTrajectoryCountForStop;
682 // ###########################################################################################################
683 j["splineSampleInterval"] = _splines.sampleInterval;
684 j["roseStepLengthMax"] = _roseStepLengthMax;
685 j["gravitationModel"] = _gravitationModel;
686 j["coriolisAccelerationEnabled"] = _coriolisAccelerationEnabled;
687 j["centrifgalAccelerationEnabled"] = _centrifgalAccelerationEnabled;
688 j["angularRateEarthRotationEnabled"] = _angularRateEarthRotationEnabled;
689 j["angularRateTransportRateEnabled"] = _angularRateTransportRateEnabled;
690 // ###########################################################################################################
691 j["Imu"] = Imu::save();
692
693 return j;
694}
695
696void NAV::ImuSimulator::restore(json const& j)
697{
698 LOG_TRACE("{}: called", nameId());
699
700 if (j.contains("startTimeSource"))
701 {
702 j.at("startTimeSource").get_to(_startTimeSource);
703 }
704 if (j.contains("startTimeEditFormat"))
705 {
706 j.at("startTimeEditFormat").get_to(_startTimeEditFormat);
707 }
708 if (j.contains("startTime"))
709 {
710 j.at("startTime").get_to(_startTime);
711 }
712 // ###########################################################################################################
713 if (j.contains("imuInternalFrequency"))
714 {
715 j.at("imuInternalFrequency").get_to(_imuInternalFrequency);
716 }
717 if (j.contains("imuFrequency"))
718 {
719 j.at("imuFrequency").get_to(_imuFrequency);
720 }
721 if (j.contains("gnssFrequency"))
722 {
723 j.at("gnssFrequency").get_to(_gnssFrequency);
724 }
725 // ###########################################################################################################
726 if (j.contains("trajectoryType"))
727 {
728 j.at("trajectoryType").get_to(_trajectoryType);
729
730 if (_trajectoryType == TrajectoryType::Csv && inputPins.empty())
731 {
732 CreateInputPin(CsvData::type().c_str(), Pin::Type::Object, { CsvData::type() });
733 }
734 else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty())
735 {
736 DeleteInputPin(0);
737 }
738 }
739 if (j.contains("startPosition"))
740 {
741 j.at("startPosition").get_to(_startPosition);
742 }
743 if (j.contains("fixedTrajectoryStartOrientation"))
744 {
745 j.at("fixedTrajectoryStartOrientation").get_to(_fixedTrajectoryStartOrientation);
746 }
747 if (j.contains("n_linearTrajectoryStartVelocity"))
748 {
749 j.at("n_linearTrajectoryStartVelocity").get_to(_n_linearTrajectoryStartVelocity);
750 }
751 if (j.contains("circularHarmonicFrequency"))
752 {
753 j.at("circularHarmonicFrequency").get_to(_circularHarmonicFrequency);
754 }
755 if (j.contains("circularHarmonicAmplitudeFactor"))
756 {
757 j.at("circularHarmonicAmplitudeFactor").get_to(_circularHarmonicAmplitudeFactor);
758 }
759 if (j.contains("trajectoryHorizontalSpeed"))
760 {
761 j.at("trajectoryHorizontalSpeed").get_to(_trajectoryHorizontalSpeed);
762 }
763 if (j.contains("trajectoryVerticalSpeed"))
764 {
765 j.at("trajectoryVerticalSpeed").get_to(_trajectoryVerticalSpeed);
766 }
767 if (j.contains("trajectoryRadius"))
768 {
769 j.at("trajectoryRadius").get_to(_trajectoryRadius);
770 }
771 if (j.contains("trajectoryRotationAngle"))
772 {
773 j.at("trajectoryRotationAngle").get_to(_trajectoryRotationAngle);
774 }
775 if (j.contains("trajectoryDirection"))
776 {
777 j.at("trajectoryDirection").get_to(_trajectoryDirection);
778 }
779 if (j.contains("rosePetNum"))
780 {
781 j.at("rosePetNum").get_to(_rosePetNum);
782 }
783 if (j.contains("rosePetDenom"))
784 {
785 j.at("rosePetDenom").get_to(_rosePetDenom);
786 }
787 // ###########################################################################################################
788 if (j.contains("simulationStopCondition"))
789 {
790 j.at("simulationStopCondition").get_to(_simulationStopCondition);
791 }
792 if (j.contains("simulationDuration"))
793 {
794 j.at("simulationDuration").get_to(_simulationDuration);
795 }
796 if (j.contains("linearTrajectoryDistanceForStop"))
797 {
798 j.at("linearTrajectoryDistanceForStop").get_to(_linearTrajectoryDistanceForStop);
799 }
800 if (j.contains("circularTrajectoryCircleCountForStop"))
801 {
802 j.at("circularTrajectoryCircleCountForStop").get_to(_circularTrajectoryCircleCountForStop);
803 }
804 if (j.contains("roseTrajectoryCountForStop"))
805 {
806 j.at("roseTrajectoryCountForStop").get_to(_roseTrajectoryCountForStop);
807 }
808 // ###########################################################################################################
809 if (j.contains("splineSampleInterval"))
810 {
811 j.at("splineSampleInterval").get_to(_splines.sampleInterval);
812 }
813 if (j.contains("roseStepLengthMax"))
814 {
815 j.at("roseStepLengthMax").get_to(_roseStepLengthMax);
816 }
817 if (j.contains("gravitationModel"))
818 {
819 j.at("gravitationModel").get_to(_gravitationModel);
820 }
821 if (j.contains("coriolisAccelerationEnabled"))
822 {
823 j.at("coriolisAccelerationEnabled").get_to(_coriolisAccelerationEnabled);
824 }
825 if (j.contains("centrifgalAccelerationEnabled"))
826 {
827 j.at("centrifgalAccelerationEnabled").get_to(_centrifgalAccelerationEnabled);
828 }
829 if (j.contains("angularRateEarthRotationEnabled"))
830 {
831 j.at("angularRateEarthRotationEnabled").get_to(_angularRateEarthRotationEnabled);
832 }
833 if (j.contains("angularRateTransportRateEnabled"))
834 {
835 j.at("angularRateTransportRateEnabled").get_to(_angularRateTransportRateEnabled);
836 }
837 // ###########################################################################################################
838 if (j.contains("Imu"))
839 {
840 Imu::restore(j.at("Imu"));
841 }
842}
843
844std::optional<NAV::InsTime> NAV::ImuSimulator::getTimeFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const // NOLINT(readability-convert-member-functions-to-static)
845{
846 auto gpsCycleIter = std::ranges::find(description, "GpsCycle");
847 auto gpsWeekIter = std::ranges::find(description, "GpsWeek");
848 auto gpsTowIter = std::ranges::find(description, "GpsToW [s]");
849 if (gpsCycleIter != description.end() && gpsWeekIter != description.end() && gpsTowIter != description.end())
850 {
851 const auto* gpsCycle = std::get_if<double>(&line.at(static_cast<size_t>(gpsCycleIter - description.begin())));
852 const auto* gpsWeek = std::get_if<double>(&line.at(static_cast<size_t>(gpsWeekIter - description.begin())));
853 const auto* gpsTow = std::get_if<double>(&line.at(static_cast<size_t>(gpsTowIter - description.begin())));
854 if (gpsCycle && gpsWeek && gpsTow)
855 {
856 return InsTime{ static_cast<int32_t>(*gpsCycle), static_cast<int32_t>(*gpsWeek), *gpsTow };
857 }
858 }
859 else
860 {
861 auto yearUTCIter = std::ranges::find(description, "YearUTC");
862 auto monthUTCIter = std::ranges::find(description, "MonthUTC");
863 auto dayUTCIter = std::ranges::find(description, "DayUTC");
864 auto hourUTCIter = std::ranges::find(description, "HourUTC");
865 auto minUTCIter = std::ranges::find(description, "MinUTC");
866 auto secUTCIter = std::ranges::find(description, "SecUTC");
867 if (yearUTCIter != description.end() && monthUTCIter != description.end() && dayUTCIter != description.end()
868 && hourUTCIter != description.end() && minUTCIter != description.end() && secUTCIter != description.end())
869 {
870 const auto* yearUTC = std::get_if<double>(&line.at(static_cast<size_t>(yearUTCIter - description.begin())));
871 const auto* monthUTC = std::get_if<double>(&line.at(static_cast<size_t>(monthUTCIter - description.begin())));
872 const auto* dayUTC = std::get_if<double>(&line.at(static_cast<size_t>(dayUTCIter - description.begin())));
873 const auto* hourUTC = std::get_if<double>(&line.at(static_cast<size_t>(hourUTCIter - description.begin())));
874 const auto* minUTC = std::get_if<double>(&line.at(static_cast<size_t>(minUTCIter - description.begin())));
875 const auto* secUTC = std::get_if<double>(&line.at(static_cast<size_t>(secUTCIter - description.begin())));
876 if (yearUTC && monthUTC && dayUTC && hourUTC && minUTC && secUTC)
877 {
878 return InsTime{
879 static_cast<uint16_t>(*yearUTC), static_cast<uint16_t>(*monthUTC), static_cast<uint16_t>(*dayUTC),
880 static_cast<uint16_t>(*hourUTC), static_cast<uint16_t>(*minUTC), *secUTC, UTC
881 };
882 }
883 }
884 }
885
886 LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the time.", nameId());
887 return std::nullopt;
888}
889
890std::optional<Eigen::Vector3d> NAV::ImuSimulator::e_getPositionFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const // NOLINT(readability-convert-member-functions-to-static)
891{
892 auto posXIter = std::ranges::find(description, "X-ECEF [m]");
893 auto posYIter = std::ranges::find(description, "Y-ECEF [m]");
894 auto posZIter = std::ranges::find(description, "Z-ECEF [m]");
895 if (posXIter != description.end() && posYIter != description.end() && posZIter != description.end())
896 {
897 const auto* posX = std::get_if<double>(&line.at(static_cast<size_t>(posXIter - description.begin())));
898 const auto* posY = std::get_if<double>(&line.at(static_cast<size_t>(posYIter - description.begin())));
899 const auto* posZ = std::get_if<double>(&line.at(static_cast<size_t>(posZIter - description.begin())));
900 if (posX && posY && posZ && !std::isnan(*posX) && !std::isnan(*posY) && !std::isnan(*posZ))
901 {
902 return Eigen::Vector3d{ *posX, *posY, *posZ };
903 }
904 }
905 else
906 {
907 auto latIter = std::ranges::find(description, "Latitude [deg]");
908 auto lonIter = std::ranges::find(description, "Longitude [deg]");
909 auto altIter = std::ranges::find(description, "Altitude [m]");
910 if (latIter != description.end() && lonIter != description.end() && altIter != description.end())
911 {
912 const auto* lat = std::get_if<double>(&line.at(static_cast<size_t>(latIter - description.begin())));
913 const auto* lon = std::get_if<double>(&line.at(static_cast<size_t>(lonIter - description.begin())));
914 const auto* alt = std::get_if<double>(&line.at(static_cast<size_t>(altIter - description.begin())));
915 if (lat && lon && alt && !std::isnan(*lat) && !std::isnan(*lon) && !std::isnan(*alt))
916 {
917 return trafo::lla2ecef_WGS84(Eigen::Vector3d(deg2rad(*lat), deg2rad(*lon), *alt));
918 }
919 }
920 }
921
922 LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the position.", nameId());
923 return std::nullopt;
924}
925
926std::optional<Eigen::Quaterniond> NAV::ImuSimulator::n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine& line, const std::vector<std::string>& description) const
927{
928 auto rollIter = std::ranges::find(description, "Roll [deg]");
929 auto pitchIter = std::ranges::find(description, "Pitch [deg]");
930 auto yawIter = std::ranges::find(description, "Yaw [deg]");
931 if (rollIter != description.end() && pitchIter != description.end() && yawIter != description.end())
932 {
933 const auto* roll = std::get_if<double>(&line.at(static_cast<size_t>(rollIter - description.begin())));
934 const auto* pitch = std::get_if<double>(&line.at(static_cast<size_t>(pitchIter - description.begin())));
935 const auto* yaw = std::get_if<double>(&line.at(static_cast<size_t>(yawIter - description.begin())));
936 if (roll && pitch && yaw && !std::isnan(*roll) && !std::isnan(*pitch) && !std::isnan(*yaw))
937 {
938 return trafo::n_Quat_b(deg2rad(*roll), deg2rad(*pitch), deg2rad(*yaw));
939 }
940 }
941 else
942 {
943 auto quatWIter = std::ranges::find(description, "n_Quat_b w");
944 auto quatXIter = std::ranges::find(description, "n_Quat_b x");
945 auto quatYIter = std::ranges::find(description, "n_Quat_b y");
946 auto quatZIter = std::ranges::find(description, "n_Quat_b z");
947 if (quatWIter != description.end() && quatXIter != description.end() && quatYIter != description.end() && quatZIter != description.end())
948 {
949 const auto* w = std::get_if<double>(&line.at(static_cast<size_t>(quatWIter - description.begin())));
950 const auto* x = std::get_if<double>(&line.at(static_cast<size_t>(quatXIter - description.begin())));
951 const auto* y = std::get_if<double>(&line.at(static_cast<size_t>(quatYIter - description.begin())));
952 const auto* z = std::get_if<double>(&line.at(static_cast<size_t>(quatZIter - description.begin())));
953 if (w && x && y && z && !std::isnan(*w) && !std::isnan(*x) && !std::isnan(*y) && !std::isnan(*z))
954 {
955 return Eigen::Quaterniond{ *w, *x, *y, *z };
956 }
957 }
958 }
959
960 LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the attitude.", nameId());
961 return std::nullopt;
962}
963
964bool NAV::ImuSimulator::initializeSplines()
965{
966 std::vector<long double> splineTime;
967
968 auto unwrapAngle = [](auto angle, auto prevAngle, auto rangeMax) {
969 auto x = angle - prevAngle;
970 x = std::fmod(x + rangeMax, 2 * rangeMax);
971 if (x < 0)
972 {
973 x += 2 * rangeMax;
974 }
975 x -= rangeMax;
976
977 return prevAngle + x;
978 };
979
980 if (_trajectoryType == TrajectoryType::Fixed)
981 {
982 splineTime.push_back(-30.0); // 10 seconds in the past; simply to define the derivative at zero seconds
983 splineTime.push_back(-20.0); // 10 seconds in the past; simply to define the derivative at zero seconds
984 splineTime.push_back(-10.0); // 10 seconds in the past; simply to define the derivative at zero seconds
985 splineTime.push_back(0.0);
986 splineTime.push_back(_simulationDuration);
987 splineTime.push_back(_simulationDuration + 10.0); // 10 seconds past simulation end; simply to define the derivative at end node
988 splineTime.push_back(_simulationDuration + 20.0); // 10 seconds past simulation end; simply to define the derivative at end node
989 splineTime.push_back(_simulationDuration + 30.0); // 10 seconds past simulation end; simply to define the derivative at end node
990
991 Eigen::Vector3d e_startPosition = _startPosition.e_position.cast<double>();
992 std::vector<long double> X(splineTime.size(), e_startPosition[0]);
993 std::vector<long double> Y(splineTime.size(), e_startPosition[1]);
994 std::vector<long double> Z(splineTime.size(), e_startPosition[2]);
995 std::vector<long double> Roll(splineTime.size(), _fixedTrajectoryStartOrientation.x());
996 std::vector<long double> Pitch(splineTime.size(), _fixedTrajectoryStartOrientation.y());
997 std::vector<long double> Yaw(splineTime.size(), _fixedTrajectoryStartOrientation.z());
998
999 _splines.x.setPoints(splineTime, X);
1000 _splines.y.setPoints(splineTime, Y);
1001 _splines.z.setPoints(splineTime, Z);
1002
1003 _splines.roll.setPoints(splineTime, Roll);
1004 _splines.pitch.setPoints(splineTime, Pitch);
1005 _splines.yaw.setPoints(splineTime, Yaw);
1006 }
1007 else if (_trajectoryType == TrajectoryType::Linear)
1008 {
1009 double roll = 0.0;
1010 double pitch = _n_linearTrajectoryStartVelocity.head<2>().norm() > 1e-8 ? calcPitchFromVelocity(_n_linearTrajectoryStartVelocity) : 0;
1011 double yaw = _n_linearTrajectoryStartVelocity.head<2>().norm() > 1e-8 ? calcYawFromVelocity(_n_linearTrajectoryStartVelocity) : 0;
1012 const auto& e_startPosition = _startPosition.e_position;
1013
1014 size_t nOverhead = static_cast<size_t>(std::round(1.0 / _splines.sampleInterval)) + 1;
1015
1016 splineTime = std::vector<long double>(nOverhead, 0.0);
1017 std::vector<long double> splineX(nOverhead, e_startPosition[0]);
1018 std::vector<long double> splineY(nOverhead, e_startPosition[1]);
1019 std::vector<long double> splineZ(nOverhead, e_startPosition[2]);
1020 std::vector<long double> splineRoll(nOverhead, roll);
1021 std::vector<long double> splinePitch(nOverhead, pitch);
1022 std::vector<long double> splineYaw(nOverhead, yaw);
1023
1024 // @brief Calculates the derivative of the curvilinear position and velocity
1025 // @param[in] y [𝜙, λ, h, v_N, v_E, v_D]^T Latitude, longitude, altitude, velocity NED in [rad, rad, m, m/s, m/s, m/s]
1026 // @param[in] n_acceleration Acceleration in local-navigation frame coordinates [m/s^s]
1027 // @return The curvilinear position and velocity derivatives ∂/∂t [𝜙, λ, h, v_N, v_E, v_D]^T
1028 auto f = [](const Eigen::Vector<double, 6>& y, const Eigen::Vector3d& n_acceleration) {
1029 Eigen::Vector<double, 6> y_dot;
1030 y_dot << lla_calcTimeDerivativeForPosition(y.tail<3>(), // Velocity with respect to the Earth in local-navigation frame coordinates [m/s]
1031 y(0), // 𝜙 Latitude in [rad]
1032 y(2), // h Altitude in [m]
1033 calcEarthRadius_N(y(0)), // North/South (meridian) earth radius [m]
1034 calcEarthRadius_E(y(0))), // East/West (prime vertical) earth radius [m]
1035 n_acceleration;
1036
1037 return y_dot;
1038 };
1039
1040 Eigen::Vector3d lla_lastPosition = _startPosition.latLonAlt();
1041 for (size_t i = 2; i <= nOverhead; i++) // Calculate one second backwards
1042 {
1043 Eigen::Vector<double, 6> y; // [𝜙, λ, h, v_N, v_E, v_D]^T
1044 y << lla_lastPosition,
1045 _n_linearTrajectoryStartVelocity;
1046
1047 y += -_splines.sampleInterval * f(y, Eigen::Vector3d::Zero());
1048 lla_lastPosition = y.head<3>();
1049
1050 Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_lastPosition);
1051
1052 splineTime.at(nOverhead - i) = -_splines.sampleInterval * static_cast<double>(i - 1);
1053 splineX.at(nOverhead - i) = e_position(0);
1054 splineY.at(nOverhead - i) = e_position(1);
1055 splineZ.at(nOverhead - i) = e_position(2);
1056 }
1057
1058 lla_lastPosition = _startPosition.latLonAlt();
1059 for (size_t i = 1;; i++)
1060 {
1061 Eigen::Vector<double, 6> y; // [𝜙, λ, h, v_N, v_E, v_D]^T
1062 y << lla_lastPosition,
1063 _n_linearTrajectoryStartVelocity;
1064
1065 y += _splines.sampleInterval * f(y, Eigen::Vector3d::Zero());
1066 lla_lastPosition = y.head<3>();
1067
1068 auto e_position = trafo::lla2ecef_WGS84(lla_lastPosition);
1069
1070 auto simTime = _splines.sampleInterval * static_cast<double>(i);
1071 splineTime.push_back(simTime);
1072 splineX.push_back(e_position(0));
1073 splineY.push_back(e_position(1));
1074 splineZ.push_back(e_position(2));
1075 splineRoll.push_back(roll);
1076 splinePitch.push_back(pitch);
1077 splineYaw.push_back(yaw);
1078
1079 if (_simulationStopCondition == StopCondition::Duration
1080 && simTime > _simulationDuration + 1.0)
1081 {
1082 break;
1083 }
1084 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1085 {
1086 auto horizontalDistance = calcGeographicalDistance(_startPosition.latitude(), _startPosition.longitude(),
1087 lla_lastPosition(0), lla_lastPosition(1));
1088 auto distance = std::sqrt(std::pow(horizontalDistance, 2) + std::pow(_startPosition.altitude() - lla_lastPosition(2), 2))
1089 - _n_linearTrajectoryStartVelocity.norm() * 1.0;
1090 if (distance > _linearTrajectoryDistanceForStop)
1091 {
1092 break;
1093 }
1094 }
1095 }
1096
1097 _splines.x.setPoints(splineTime, splineX);
1098 _splines.y.setPoints(splineTime, splineY);
1099 _splines.z.setPoints(splineTime, splineZ);
1100
1101 _splines.roll.setPoints(splineTime, splineRoll);
1102 _splines.pitch.setPoints(splineTime, splinePitch);
1103 _splines.yaw.setPoints(splineTime, splineYaw);
1104 }
1105 else if (_trajectoryType == TrajectoryType::Circular)
1106 {
1107 double simDuration{};
1108 if (_simulationStopCondition == StopCondition::Duration)
1109 {
1110 simDuration = _simulationDuration;
1111 }
1112 else if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1113 {
1114 double omega = _trajectoryHorizontalSpeed / _trajectoryRadius;
1115 simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega;
1116 }
1117
1118 constexpr double OFFSET = 1.0; // [s]
1119
1120 for (size_t i = 0; i <= static_cast<size_t>(std::round((simDuration + 2.0 * OFFSET) / _splines.sampleInterval)); i++)
1121 {
1122 splineTime.push_back(_splines.sampleInterval * static_cast<double>(i) - OFFSET);
1123 }
1124 LOG_DATA("{}: Sim Time [{:.3f}, {:.3f}] with dt = {:.3f} (simDuration = {:.3f})", nameId(),
1125 static_cast<double>(splineTime.front()), static_cast<double>(splineTime.back()), static_cast<double>(splineTime.at(1) - splineTime.at(0)), simDuration);
1126
1127 std::vector<long double> splineX(splineTime.size());
1128 std::vector<long double> splineY(splineTime.size());
1129 std::vector<long double> splineZ(splineTime.size());
1130
1131 Eigen::Vector3d e_origin = _startPosition.e_position;
1132 Eigen::Vector3d lla_origin = _startPosition.latLonAlt();
1133
1134 Eigen::Quaterniond e_quatCenter_n = trafo::e_Quat_n(lla_origin(0), lla_origin(1));
1135
1136 for (uint64_t i = 0; i < splineTime.size(); i++)
1137 {
1138 auto phi = _trajectoryHorizontalSpeed * static_cast<double>(splineTime[i]) / _trajectoryRadius; // Angle of the current point on the circle
1139 phi *= _trajectoryDirection == Direction::CW ? -1 : 1;
1140 phi += _trajectoryRotationAngle;
1141 // LOG_DATA("{}: [t={:.3f}] phi = {:.3f}°", nameId(), static_cast<double>(splineTime.at(i)), rad2deg(phi));
1142
1143 Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::sin(phi) * (1 + _circularHarmonicAmplitudeFactor * std::sin(phi * static_cast<double>(_circularHarmonicFrequency))), // [m]
1144 _trajectoryRadius * std::cos(phi) * (1 + _circularHarmonicAmplitudeFactor * std::sin(phi * static_cast<double>(_circularHarmonicFrequency))), // [m]
1145 -_trajectoryVerticalSpeed * static_cast<double>(splineTime[i]) }; // [m]
1146
1147 Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition;
1148
1149 Eigen::Vector3d e_position = e_origin + e_relativePosition;
1150
1151 splineX[i] = e_position[0];
1152 splineY[i] = e_position[1];
1153 splineZ[i] = e_position[2];
1154 }
1155
1156 _splines.x.setPoints(splineTime, splineX);
1157 _splines.y.setPoints(splineTime, splineY);
1158 _splines.z.setPoints(splineTime, splineZ);
1159 }
1160 else if (_trajectoryType == TrajectoryType::RoseFigure)
1161 {
1162 // Formulas and notation for the rose figure from https://en.wikipedia.org/wiki/Rose_(mathematics)
1163
1164 // Adjusting the integration bounds needs a factor * PI
1165 // | d \ n | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
1166 // | ------ | --- | --- | --- | --- | --- | --- | --- |
1167 // | 1 | 1 | 2 | 1 | 2 | 1 | 2 | 1 |
1168 // | 2 | 4 | 1 | 4 | 2 | 4 | 1 | 4 |
1169 // | 3 | 3 | 6 | 1 | 6 | 3 | 2 | 3 |
1170 // | 4 | 8 | 4 | 8 | 1 | 8 | 4 | 8 |
1171 // | 5 | 5 | 10 | 5 | 10 | 1 | 10 | 5 |
1172 // | 6 | 12 | 3 | 4 | 6 | 12 | 1 | 12 |
1173 // | 7 | 7 | 14 | 7 | 14 | 7 | 14 | 1 |
1174 // | 8 | 16 | 8 | 16 | 4 | 16 | 8 | 16 |
1175 // | 9 | 9 | 18 | 3 | 18 | 9 | 6 | 9 |
1176
1177 int n = _rosePetNum;
1178 int d = _rosePetDenom;
1179
1180 for (int i = 2; i <= n; i++) // reduction of fraction ( 4/2 ==> 2/1 )
1181 {
1182 if (n % i == 0 && d % i == 0)
1183 {
1184 n /= i;
1185 d /= i;
1186 i--;
1187 }
1188 }
1189
1190 auto isOdd = [](auto a) { return static_cast<int>(a) % 2 != 0; };
1191 auto isEven = [](auto a) { return static_cast<int>(a) % 2 == 0; };
1192
1193 double integrationFactor = 0.0;
1194 if (isOdd(d))
1195 {
1196 if (isOdd(n)) { integrationFactor = static_cast<double>(d); }
1197 else { integrationFactor = 2.0 * static_cast<double>(d); }
1198 }
1199 else // if (isEven(d))
1200 {
1201 if (isEven(n)) { integrationFactor = static_cast<double>(d); }
1202 else { integrationFactor = 2.0 * static_cast<double>(d); }
1203 }
1204
1205 auto nVirtPoints = static_cast<size_t>(1.0 / (_roseStepLengthMax / 50.0));
1206 splineTime.resize(nVirtPoints); // Preallocate points to make the spline start at the right point
1207 std::vector<long double> splineX(splineTime.size());
1208 std::vector<long double> splineY(splineTime.size());
1209 std::vector<long double> splineZ(splineTime.size());
1210
1211 Eigen::Vector3d e_origin = trafo::lla2ecef_WGS84(_startPosition.latLonAlt());
1212
1213 Eigen::Quaterniond e_quatCenter_n = trafo::e_Quat_n(_startPosition.latLonAlt()(0), _startPosition.latLonAlt()(1));
1214
1215 double lengthOld = -_roseStepLengthMax / 2.0; // n-1 length
1216 double dPhi = 0.001; // Angle step size
1217 double maxPhi = std::numeric_limits<double>::infinity(); // Interval for integration depending on selected stop criteria
1218 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1219 {
1220 maxPhi = _roseTrajectoryCountForStop * integrationFactor * M_PI;
1221 }
1222
1223 // k = n/d
1224 double roseK = static_cast<double>(_rosePetNum) / static_cast<double>(_rosePetDenom);
1225
1226 _roseSimDuration = 0.0;
1227
1228 // We cannot input negative values or zero
1229 for (double phi = dPhi; phi <= maxPhi + static_cast<double>(nVirtPoints) * dPhi; phi += dPhi) // NOLINT(clang-analyzer-security.FloatLoopCounter, cert-flp30-c)
1230 {
1231 double length = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * phi, 1.0 - std::pow(roseK, 2.0));
1232 double dL = length - lengthOld;
1233
1234 if (dL > _roseStepLengthMax)
1235 {
1236 phi -= dPhi;
1237 dPhi /= 2.0;
1238 continue;
1239 }
1240 if (dL < _roseStepLengthMax / 3.0) // Avoid also too small steps
1241 {
1242 phi -= dPhi;
1243 dPhi *= 1.5;
1244 continue;
1245 }
1246 lengthOld = length;
1247
1248 double time = length / _trajectoryHorizontalSpeed;
1249 splineTime.push_back(time);
1250 Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::cos(roseK * phi) * std::sin(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1251 _trajectoryRadius * std::cos(roseK * phi) * std::cos(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1252 -_trajectoryVerticalSpeed * time }; // [m]
1253
1254 Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition;
1255 Eigen::Vector3d e_position = e_origin + e_relativePosition;
1256
1257 // LOG_DATA("{}: t={: 8.3f}s | l={:8.6}m | phi={:6.3f}", nameId(), time, length, rad2deg(phi));
1258
1259 splineX.push_back(e_position[0]);
1260 splineY.push_back(e_position[1]);
1261 splineZ.push_back(e_position[2]);
1262
1263 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses && std::abs(maxPhi - phi) < 1.0 * dPhi)
1264 {
1265 LOG_TRACE("{}: Rose figure simulation duration: {:8.6}s | l={:8.6}m", nameId(), time, length);
1266 _roseSimDuration = time;
1267 }
1268 else if (_simulationStopCondition == StopCondition::Duration && _roseSimDuration == 0.0 && time > _simulationDuration)
1269 {
1270 _roseSimDuration = _simulationDuration;
1271 maxPhi = phi;
1272 }
1273 }
1274
1275 maxPhi = integrationFactor * M_PI + 1e-15; // For exactly 2*pi the elliptical integral is failing. Bug in the function.
1276 // LOG_DATA("maxPhi {:6.2f}", rad2deg(maxPhi));
1277 double endLength = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * maxPhi, 1.0 - std::pow(roseK, 2.0));
1278 // LOG_DATA("endLength {:8.6}m", endLength);
1279 for (size_t i = 0; i < nVirtPoints; i++)
1280 {
1281 double phi = maxPhi - static_cast<double>(i) * dPhi;
1282 double length = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * phi, 1.0 - std::pow(roseK, 2.0));
1283 double time = (length - endLength) / _trajectoryHorizontalSpeed;
1284 splineTime[nVirtPoints - i - 1] = time;
1285
1286 Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::cos(roseK * phi) * std::sin(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1287 _trajectoryRadius * std::cos(roseK * phi) * std::cos(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1288 -_trajectoryVerticalSpeed * time }; // [m]
1289
1290 Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition;
1291 Eigen::Vector3d e_position = e_origin + e_relativePosition;
1292
1293 // LOG_DATA("{}: t={: 8.3f}s | l={:8.6}m | phi={:6.3f}", nameId(), time, length, rad2deg(phi));
1294
1295 splineX[nVirtPoints - i - 1] = e_position[0];
1296 splineY[nVirtPoints - i - 1] = e_position[1];
1297 splineZ[nVirtPoints - i - 1] = e_position[2];
1298 }
1299
1300 _splines.x.setPoints(splineTime, splineX);
1301 _splines.y.setPoints(splineTime, splineY);
1302 _splines.z.setPoints(splineTime, splineZ);
1303 }
1304 else if (_trajectoryType == TrajectoryType::Csv)
1305 {
1306 if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV);
1307 csvData && csvData->v->lines.size() >= 2)
1308 {
1309 if (auto startTime = getTimeFromCsvLine(csvData->v->lines.front(), csvData->v->description))
1310 {
1311 if (!startTime) { return false; }
1312 _startTime = *startTime;
1313 }
1314
1315 constexpr size_t nVirtPoints = 0;
1316 splineTime.resize(nVirtPoints); // Preallocate points to make the spline start at the right point
1317 std::vector<long double> splineX(splineTime.size());
1318 std::vector<long double> splineY(splineTime.size());
1319 std::vector<long double> splineZ(splineTime.size());
1320 std::vector<long double> splineRoll(splineTime.size());
1321 std::vector<long double> splinePitch(splineTime.size());
1322 std::vector<long double> splineYaw(splineTime.size());
1323
1324 for (size_t i = 0; i < csvData->v->lines.size(); i++)
1325 {
1326 auto insTime = getTimeFromCsvLine(csvData->v->lines[i], csvData->v->description);
1327 if (!insTime)
1328 {
1329 LOG_ERROR("{}: Data in time column not found in line {}", nameId(), i + 2);
1330 return false;
1331 }
1332 // LOG_DATA("{}: Time {}", nameId(), *insTime);
1333 auto time = static_cast<double>((*insTime - _startTime).count());
1334
1335 auto e_pos = e_getPositionFromCsvLine(csvData->v->lines[i], csvData->v->description);
1336 if (!e_pos)
1337 {
1338 LOG_ERROR("{}: Data in position column not found in line {}", nameId(), i + 2);
1339 return false;
1340 }
1341 // LOG_DATA("{}: e_pos {}", nameId(), *e_pos);
1342
1343 auto n_Quat_b = n_getAttitudeQuaternionFromCsvLine_b(csvData->v->lines[i], csvData->v->description);
1344 if (!n_Quat_b)
1345 {
1346 LOG_ERROR("{}: Data in attitude column not found in line {}", nameId(), i + 2);
1347 return false;
1348 }
1349 // LOG_DATA("{}: n_Quat_b {}", nameId(), *n_Quat_b);
1350
1351 splineTime.push_back(time);
1352 splineX.push_back(e_pos->x());
1353 splineY.push_back(e_pos->y());
1354 splineZ.push_back(e_pos->z());
1355
1356 auto rpy = trafo::quat2eulerZYX(*n_Quat_b);
1357 // LOG_DATA("{}: RPY {} [deg] (from CSV)", nameId(), rad2deg(rpy).transpose());
1358 splineRoll.push_back(i > 0 ? unwrapAngle(rpy(0), splineRoll.back(), M_PI) : rpy(0));
1359 splinePitch.push_back(i > 0 ? unwrapAngle(rpy(1), splinePitch.back(), M_PI_2) : rpy(1));
1360 splineYaw.push_back(i > 0 ? unwrapAngle(rpy(2), splineYaw.back(), M_PI) : rpy(2));
1361 // LOG_DATA("{}: R {}, P {}, Y {} [deg] (in Spline)", nameId(), rad2deg(splineRoll.back()), rad2deg(splinePitch.back()), rad2deg(splineYaw.back()));
1362 }
1363 _csvDuration = static_cast<double>(splineTime.back());
1364
1365 auto dt = static_cast<double>(splineTime[nVirtPoints + 1] - splineTime[nVirtPoints]);
1366 for (size_t i = 0; i < nVirtPoints; i++)
1367 {
1368 double h = 0.001;
1369 splineTime[nVirtPoints - i - 1] = splineTime[nVirtPoints - i] - h;
1370 splineX[nVirtPoints - i - 1] = splineX[nVirtPoints - i] - h * (splineX[nVirtPoints + 1] - splineX[nVirtPoints]) / dt;
1371 splineY[nVirtPoints - i - 1] = splineY[nVirtPoints - i] - h * (splineY[nVirtPoints + 1] - splineY[nVirtPoints]) / dt;
1372 splineZ[nVirtPoints - i - 1] = splineZ[nVirtPoints - i] - h * (splineZ[nVirtPoints + 1] - splineZ[nVirtPoints]) / dt;
1373 splineRoll[nVirtPoints - i - 1] = splineRoll[nVirtPoints - i] - h * (splineRoll[nVirtPoints + 1] - splineRoll[nVirtPoints]) / dt;
1374 splinePitch[nVirtPoints - i - 1] = splinePitch[nVirtPoints - i] - h * (splinePitch[nVirtPoints + 1] - splinePitch[nVirtPoints]) / dt;
1375 splineYaw[nVirtPoints - i - 1] = splineYaw[nVirtPoints - i] - h * (splineYaw[nVirtPoints + 1] - splineYaw[nVirtPoints]) / dt;
1376 splineTime.push_back(splineTime[splineX.size() - 1] + h);
1377 splineX.push_back(splineX[splineX.size() - 1] + h * (splineX[splineX.size() - 1] - splineX[splineX.size() - 2]) / dt);
1378 splineY.push_back(splineY[splineY.size() - 1] + h * (splineY[splineY.size() - 1] - splineY[splineY.size() - 2]) / dt);
1379 splineZ.push_back(splineZ[splineZ.size() - 1] + h * (splineZ[splineZ.size() - 1] - splineZ[splineZ.size() - 2]) / dt);
1380 splineRoll.push_back(splineRoll[splineRoll.size() - 1] + h * (splineRoll[splineRoll.size() - 1] - splineRoll[splineRoll.size() - 2]) / dt);
1381 splinePitch.push_back(splinePitch[splinePitch.size() - 1] + h * (splinePitch[splinePitch.size() - 1] - splinePitch[splinePitch.size() - 2]) / dt);
1382 splineYaw.push_back(splineYaw[splineYaw.size() - 1] + h * (splineYaw[splineYaw.size() - 1] - splineYaw[splineYaw.size() - 2]) / dt);
1383 }
1384
1385 _splines.x.setPoints(splineTime, splineX);
1386 _splines.y.setPoints(splineTime, splineY);
1387 _splines.z.setPoints(splineTime, splineZ);
1388
1389 _splines.roll.setPoints(splineTime, splineRoll);
1390 _splines.pitch.setPoints(splineTime, splinePitch);
1391 _splines.yaw.setPoints(splineTime, splineYaw);
1392 }
1393 else
1394 {
1395 LOG_ERROR("{}: Can't calculate the data without a connected CSV file with at least two datasets", nameId());
1396 return false;
1397 }
1398 }
1399
1400 if (_trajectoryType == TrajectoryType::Circular || _trajectoryType == TrajectoryType::RoseFigure)
1401 {
1402 std::vector<long double> splineRoll(splineTime.size());
1403 std::vector<long double> splinePitch(splineTime.size());
1404 std::vector<long double> splineYaw(splineTime.size());
1405
1406 for (uint64_t i = 0; i < splineTime.size(); i++)
1407 {
1408 Eigen::Vector3d e_pos{ static_cast<double>(_splines.x(splineTime[i])),
1409 static_cast<double>(_splines.y(splineTime[i])),
1410 static_cast<double>(_splines.z(splineTime[i])) };
1411 Eigen::Vector3d e_vel{ static_cast<double>(_splines.x.derivative(1, splineTime[i])),
1412 static_cast<double>(_splines.y.derivative(1, splineTime[i])),
1413 static_cast<double>(_splines.z.derivative(1, splineTime[i])) };
1414
1415 Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(e_pos);
1416 Eigen::Vector3d n_velocity = trafo::n_Quat_e(lla_position(0), lla_position(1)) * e_vel;
1417
1418 Eigen::Vector3d e_normalVectorCenterCircle{ std::cos(_startPosition.latLonAlt()(0)) * std::cos(_startPosition.latLonAlt()(1)),
1419 std::cos(_startPosition.latLonAlt()(0)) * std::sin(_startPosition.latLonAlt()(1)),
1420 std::sin(_startPosition.latLonAlt()(0)) };
1421
1422 Eigen::Vector3d e_normalVectorCurrentPosition{ std::cos(lla_position(0)) * std::cos(lla_position(1)),
1423 std::cos(lla_position(0)) * std::sin(lla_position(1)),
1424 std::sin(lla_position(0)) };
1425
1426 auto yaw = calcYawFromVelocity(n_velocity);
1427
1428 splineYaw[i] = i > 0 ? unwrapAngle(yaw, splineYaw[i - 1], M_PI) : yaw;
1429 splineRoll[i] = 0.0;
1430 splinePitch[i] = n_velocity.head<2>().norm() > 1e-8 ? calcPitchFromVelocity(n_velocity) : 0;
1431 // LOG_DATA("{}: [t={:.3f}] yaw = {:.3f}°", nameId(), static_cast<double>(splineTime.at(i)), rad2deg(splineYaw[i]));
1432 }
1433
1434 _splines.roll.setPoints(splineTime, splineRoll);
1435 _splines.pitch.setPoints(splineTime, splinePitch);
1436 _splines.yaw.setPoints(splineTime, splineYaw);
1437 }
1438
1439 return true;
1440}
1441
1442bool NAV::ImuSimulator::initialize()
1443{
1444 LOG_TRACE("{}: called", nameId());
1445
1446 return initializeSplines();
1447}
1448
1449void NAV::ImuSimulator::deinitialize()
1450{
1451 LOG_TRACE("{}: called", nameId());
1452}
1453
1454bool NAV::ImuSimulator::resetNode()
1455{
1456 LOG_TRACE("{}: called", nameId());
1457
1458 _imuInternalUpdateCnt = 0;
1459 _imuUpdateCnt = 0;
1460 _gnssUpdateCnt = 0;
1461
1462 _imuLastUpdateTime = 0.0;
1463 _gnssLastUpdateTime = 0.0;
1464 _lla_imuLastLinearPosition = _startPosition.latLonAlt();
1465 _lla_gnssLastLinearPosition = _startPosition.latLonAlt();
1466
1467 _p_lastImuAccelerationMeas = Eigen::Vector3d::Ones() * std::nan("");
1468 _p_lastImuAngularRateMeas = Eigen::Vector3d::Ones() * std::nan("");
1469
1470 if (_trajectoryType == TrajectoryType::Csv)
1471 {
1472 if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV);
1473 csvData && !csvData->v->lines.empty())
1474 {
1475 if (auto startTime = getTimeFromCsvLine(csvData->v->lines.front(), csvData->v->description))
1476 {
1477 if (!startTime) { return false; }
1478 _startTime = *startTime;
1479 }
1480 LOG_DEBUG("{}: Start Time set to {}", nameId(), _startTime);
1481 }
1482 else
1483 {
1484 LOG_ERROR("{}: Can't reset the ImuSimulator without a connected CSV file", nameId());
1485 return false;
1486 }
1487 }
1488 else if (_startTimeSource == StartTimeSource::CurrentComputerTime)
1489 {
1490 std::time_t t = std::time(nullptr);
1491 std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe)
1492
1493 _startTime = InsTime{ static_cast<uint16_t>(now->tm_year + 1900), static_cast<uint16_t>(now->tm_mon), static_cast<uint16_t>(now->tm_mday),
1494 static_cast<uint16_t>(now->tm_hour), static_cast<uint16_t>(now->tm_min), static_cast<long double>(now->tm_sec) };
1495 LOG_DEBUG("{}: Start Time set to {}", nameId(), _startTime);
1496 }
1497
1498 return true;
1499}
1500
1501bool NAV::ImuSimulator::checkStopCondition(double time, const Eigen::Vector3d& lla_position)
1502{
1503 if (_trajectoryType == TrajectoryType::Csv)
1504 {
1505 return time > _csvDuration;
1506 }
1507 if (_simulationStopCondition == StopCondition::Duration
1508 || _trajectoryType == TrajectoryType::Fixed)
1509 {
1510 return time > _simulationDuration;
1511 }
1512 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1513 {
1514 if (_trajectoryType == TrajectoryType::Linear)
1515 {
1516 auto horizontalDistance = calcGeographicalDistance(_startPosition.latitude(), _startPosition.longitude(),
1517 lla_position(0), lla_position(1));
1518 auto distance = std::sqrt(std::pow(horizontalDistance, 2) + std::pow(_startPosition.altitude() - lla_position(2), 2));
1519 return distance > _linearTrajectoryDistanceForStop;
1520 }
1521 if (_trajectoryType == TrajectoryType::Circular)
1522 {
1523 double omega = _trajectoryHorizontalSpeed / _trajectoryRadius;
1524 double simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega;
1525 return time > simDuration;
1526 }
1527 if (_trajectoryType == TrajectoryType::RoseFigure)
1528 {
1529 return time > _roseSimDuration;
1530 }
1531 }
1532 return false;
1533}
1534
1535std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollImuObs(size_t /* pinIdx */, bool peek)
1536{
1537 double imuUpdateTime = static_cast<double>(_imuUpdateCnt) / _imuFrequency;
1538
1539 // -------------------------------------------------- Check if a stop condition is met -----------------------------------------------------
1540 if (checkStopCondition(imuUpdateTime, lla_calcPosition(imuUpdateTime)))
1541 {
1542 return nullptr;
1543 }
1544 // ------------------------------------- Early return in case of peeking to avoid heavy calculations ---------------------------------------
1545 if (peek)
1546 {
1547 auto obs = std::make_shared<NodeData>();
1548 obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime);
1549 return obs;
1550 }
1551
1552 auto obs = std::make_shared<ImuObsSimulated>(_imuPos);
1553 obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime);
1554 LOG_DATA("{}: Simulated IMU data for time [{}] (_imuUpdateCnt {}, _imuInternalUpdateCnt {})", nameId(), obs->insTime.toYMDHMS(),
1555 _imuUpdateCnt, _imuInternalUpdateCnt);
1556
1557 double dTime = 0.0;
1558 Eigen::Vector3d p_deltaVel = Eigen::Vector3d::Zero();
1559 Eigen::Vector3d p_deltaTheta = Eigen::Vector3d::Zero();
1560
1561 double imuInternalUpdateTime = 0.0;
1562 do {
1563 imuInternalUpdateTime = static_cast<double>(_imuInternalUpdateCnt) / _imuInternalFrequency - 1.0 / _imuFrequency;
1564 LOG_DATA("{}: Simulated internal IMU data for time [{}] (_imuUpdateCnt {}, _imuInternalUpdateCnt {})", nameId(),
1565 (_startTime + std::chrono::duration<double>(imuInternalUpdateTime)).toYMDHMS(), _imuUpdateCnt, _imuInternalUpdateCnt);
1566
1567 // --------------------------------------------------------- Calculation of data -----------------------------------------------------------
1568 Eigen::Vector3d lla_position = lla_calcPosition(imuInternalUpdateTime);
1569 LOG_DATA("{}: [{:8.3f}] lla_position = {}°, {}°, {} m", nameId(), imuInternalUpdateTime, rad2deg(lla_position(0)), rad2deg(lla_position(1)), lla_position(2));
1570 Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_position);
1571 Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1));
1572
1573 Eigen::Vector3d n_vel = n_calcVelocity(imuInternalUpdateTime, n_Quat_e);
1574 LOG_DATA("{}: [{:8.3f}] n_vel = {} [m/s]", nameId(), imuInternalUpdateTime, n_vel.transpose());
1575
1576 auto [roll, pitch, yaw] = calcFlightAngles(imuInternalUpdateTime);
1577 LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), imuInternalUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw));
1578
1579 Eigen::Quaterniond b_Quat_n = trafo::b_Quat_n(roll, pitch, yaw);
1580
1581 Eigen::Vector3d n_omega_ie = n_Quat_e * InsConst::e_omega_ie;
1582 LOG_DATA("{}: [{:8.3f}] n_omega_ie = {} [rad/s]", nameId(), imuInternalUpdateTime, n_omega_ie.transpose());
1583 auto R_N = calcEarthRadius_N(lla_position(0));
1584 LOG_DATA("{}: [{:8.3f}] R_N = {} [m]", nameId(), imuInternalUpdateTime, R_N);
1585 auto R_E = calcEarthRadius_E(lla_position(0));
1586 LOG_DATA("{}: [{:8.3f}] R_E = {} [m]", nameId(), imuInternalUpdateTime, R_E);
1587 Eigen::Vector3d n_omega_en = n_calcTransportRate(lla_position, n_vel, R_N, R_E);
1588 LOG_DATA("{}: [{:8.3f}] n_omega_en = {} [rad/s]", nameId(), imuInternalUpdateTime, n_omega_en.transpose());
1589
1590 // ------------------------------------------------------------ Accelerations --------------------------------------------------------------
1591
1592 // Force to keep vehicle on track
1593 Eigen::Vector3d n_trajectoryAccel = n_calcTrajectoryAccel(imuInternalUpdateTime, n_Quat_e, lla_position, n_vel);
1594 LOG_DATA("{}: [{:8.3f}] n_trajectoryAccel = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_trajectoryAccel.transpose());
1595
1596 // Measured acceleration in local-navigation frame coordinates [m/s^2]
1597 Eigen::Vector3d n_accel = n_trajectoryAccel;
1598 if (_coriolisAccelerationEnabled) // Apply Coriolis Acceleration
1599 {
1600 Eigen::Vector3d n_coriolisAcceleration = n_calcCoriolisAcceleration(n_omega_ie, n_omega_en, n_vel);
1601 LOG_DATA("{}: [{:8.3f}] n_coriolisAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_coriolisAcceleration.transpose());
1602 n_accel += n_coriolisAcceleration;
1603 }
1604
1605 // Mass attraction of the Earth (gravitation)
1606 Eigen::Vector3d n_gravitation = n_calcGravitation(lla_position, _gravitationModel);
1607 LOG_DATA("{}: [{:8.3f}] n_gravitation = {} [m/s^2] ({})", nameId(), imuInternalUpdateTime, n_gravitation.transpose(), NAV::to_string(_gravitationModel));
1608 n_accel -= n_gravitation; // Apply the local gravity vector
1609
1610 if (_centrifgalAccelerationEnabled) // Centrifugal acceleration caused by the Earth's rotation
1611 {
1612 Eigen::Vector3d e_centrifugalAcceleration = e_calcCentrifugalAcceleration(e_position);
1613 LOG_DATA("{}: [{:8.3f}] e_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, e_centrifugalAcceleration.transpose());
1614 Eigen::Vector3d n_centrifugalAcceleration = n_Quat_e * e_centrifugalAcceleration;
1615 LOG_DATA("{}: [{:8.3f}] n_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_centrifugalAcceleration.transpose());
1616 n_accel += n_centrifugalAcceleration;
1617 }
1618
1619 // Acceleration measured by the accelerometer in platform coordinates
1620 Eigen::Vector3d p_accel = _imuPos.p_quat_b() * b_Quat_n * n_accel;
1621 LOG_DATA("{}: [{:8.3f}] p_accel = {} [m/s^2]", nameId(), imuInternalUpdateTime, p_accel.transpose());
1622
1623 // ------------------------------------------------------------ Angular rates --------------------------------------------------------------
1624
1625 Eigen::Vector3d n_omega_nb = n_calcOmega_nb(imuInternalUpdateTime, Eigen::Vector3d{ roll, pitch, yaw }, b_Quat_n.conjugate());
1626
1627 // ω_ib_n = ω_in_n + ω_nb_n = (ω_ie_n + ω_en_n) + ω_nb_n
1628 Eigen::Vector3d n_omega_ib = n_omega_nb;
1629 if (_angularRateEarthRotationEnabled)
1630 {
1631 n_omega_ib += n_omega_ie;
1632 }
1633 if (_angularRateTransportRateEnabled)
1634 {
1635 n_omega_ib += n_omega_en;
1636 }
1637
1638 // ω_ib_b = b_Quat_n * ω_ib_n
1639 // = 0
1640 // ω_ip_p = p_Quat_b * (ω_ib_b + ω_bp_b) = p_Quat_b * ω_ib_b
1641 Eigen::Vector3d p_omega_ip = _imuPos.p_quat_b() * b_Quat_n * n_omega_ib;
1642 LOG_DATA("{}: [{:8.3f}] p_omega_ip = {} [rad/s]", nameId(), imuInternalUpdateTime, p_omega_ip.transpose());
1643
1644 // -------------------------------------------------- Construct the message to send out ----------------------------------------------------
1645
1646 obs->p_acceleration = p_accel.cast<double>();
1647 obs->p_angularRate = p_omega_ip.cast<double>();
1648 // obs->p_magneticField.emplace(0, 0, 0);
1649
1650 Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate();
1651
1652 obs->n_accelDynamics = n_trajectoryAccel.cast<double>();
1653 obs->n_angularRateDynamics = n_omega_nb.cast<double>();
1654
1655 obs->e_accelDynamics = (e_Quat_n * n_trajectoryAccel).cast<double>();
1656 obs->e_angularRateDynamics = (e_Quat_n * n_omega_nb).cast<double>();
1657
1658 double dt = 1.0 / _imuInternalFrequency;
1659 if (_imuInternalUpdateCnt != 0)
1660 {
1661 dTime += dt;
1662
1663 p_deltaVel += (p_accel + _p_lastImuAccelerationMeas) / 2 * dt;
1664 p_deltaTheta += (p_omega_ip + _p_lastImuAngularRateMeas) / 2 * dt;
1665 }
1666 _p_lastImuAccelerationMeas = p_accel;
1667 _p_lastImuAngularRateMeas = p_omega_ip;
1668
1669 _imuInternalUpdateCnt++;
1670 } while (imuInternalUpdateTime + 1e-6 < imuUpdateTime);
1671
1672 obs->dtime = dTime;
1673 obs->dvel = p_deltaVel.cast<double>();
1674 obs->dtheta = p_deltaTheta.cast<double>();
1675 LOG_DATA("{}: dtime = {}, dvel = {}, dtheta = {}", nameId(), obs->dtime, obs->dvel.transpose(), obs->dtheta.transpose());
1676
1677 _imuUpdateCnt++;
1678 invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
1679
1680 return obs;
1681}
1682
1683std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollPosVelAtt(size_t /* pinIdx */, bool peek)
1684{
1685 auto gnssUpdateTime = static_cast<double>(_gnssUpdateCnt) / _gnssFrequency;
1686
1687 Eigen::Vector3d lla_position = lla_calcPosition(gnssUpdateTime);
1688
1689 // -------------------------------------------------- Check if a stop condition is met -----------------------------------------------------
1690 if (checkStopCondition(gnssUpdateTime, lla_position))
1691 {
1692 return nullptr;
1693 }
1694
1695 // ------------------------------------- Early return in case of peeking to avoid heavy calculations ---------------------------------------
1696 if (peek)
1697 {
1698 auto obs = std::make_shared<NodeData>();
1699 obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime);
1700 return obs;
1701 }
1702 auto obs = std::make_shared<PosVelAtt>();
1703 obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime);
1704 LOG_DATA("{}: Simulating GNSS data for time [{}]", nameId(), obs->insTime.toYMDHMS());
1705
1706 // --------------------------------------------------------- Calculation of data -----------------------------------------------------------
1707 LOG_DATA("{}: [{:8.3f}] lla_position = {}°, {}°, {} m", nameId(), gnssUpdateTime, rad2deg(lla_position(0)), rad2deg(lla_position(1)), lla_position(2));
1708 Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1));
1709 Eigen::Vector3d n_vel = n_calcVelocity(gnssUpdateTime, n_Quat_e);
1710 LOG_DATA("{}: [{:8.3f}] n_vel = {} [m/s]", nameId(), gnssUpdateTime, n_vel.transpose());
1711 auto [roll, pitch, yaw] = calcFlightAngles(gnssUpdateTime);
1712 LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), gnssUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw));
1713
1714 // -------------------------------------------------- Construct the message to send out ----------------------------------------------------
1715
1716 obs->setPosVelAtt_n(lla_position.cast<double>(), n_vel.cast<double>(), trafo::n_Quat_b(roll, pitch, yaw).cast<double>());
1717
1718 _gnssUpdateCnt++;
1719 invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, obs);
1720
1721 return obs;
1722}
1723
1724std::array<double, 3> NAV::ImuSimulator::calcFlightAngles(double time) const
1725{
1726 return {
1727 static_cast<double>(_splines.roll(time)),
1728 static_cast<double>(_splines.pitch(time)),
1729 static_cast<double>(_splines.yaw(time))
1730 };
1731}
1732
1733Eigen::Vector3d NAV::ImuSimulator::lla_calcPosition(double time) const
1734{
1735 Eigen::Vector3d e_pos(static_cast<double>(_splines.x(time)),
1736 static_cast<double>(_splines.y(time)),
1737 static_cast<double>(_splines.z(time)));
1738 return trafo::ecef2lla_WGS84(e_pos);
1739}
1740
1741Eigen::Vector3d NAV::ImuSimulator::n_calcVelocity(double time, const Eigen::Quaterniond& n_Quat_e) const
1742{
1743 Eigen::Vector3d e_vel(static_cast<double>(_splines.x.derivative(1, time)),
1744 static_cast<double>(_splines.y.derivative(1, time)),
1745 static_cast<double>(_splines.z.derivative(1, time)));
1746 return n_Quat_e * e_vel;
1747}
1748
1749Eigen::Vector3d NAV::ImuSimulator::n_calcTrajectoryAccel(double time,
1750 const Eigen::Quaterniond& n_Quat_e,
1751 const Eigen::Vector3d& lla_position,
1752 const Eigen::Vector3d& n_velocity) const
1753{
1754 Eigen::Vector3d e_accel(static_cast<double>(_splines.x.derivative(2, time)),
1755 static_cast<double>(_splines.y.derivative(2, time)),
1756 static_cast<double>(_splines.z.derivative(2, time)));
1757 Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate();
1758 Eigen::Vector3d e_vel = e_Quat_n * n_velocity;
1759
1760 // Math: \dot{C}_n^e = C_n^e \cdot \Omega_{en}^n
1761 Eigen::Matrix3d n_DCM_dot_e = e_Quat_n.toRotationMatrix()
1762 * math::skewSymmetricMatrix(n_calcTransportRate(lla_position, n_velocity,
1763 calcEarthRadius_N(lla_position(0)),
1764 calcEarthRadius_E(lla_position(0))));
1765
1766 // Math: \dot{C}_e^n = (\dot{C}_n^e)^T
1767 Eigen::Matrix3d e_DCM_dot_n = n_DCM_dot_e.transpose();
1768
1769 // Math: a^n = \frac{\partial}{\partial t} \left( \dot{x}^n \right) = \frac{\partial}{\partial t} \left( C_e^n \cdot \dot{x}^e \right) = \dot{C}_e^n \cdot \dot{x}^e + C_e^n \cdot \ddot{x}^e
1770 return e_DCM_dot_n * e_vel + n_Quat_e * e_accel;
1771}
1772
1773Eigen::Vector3d NAV::ImuSimulator::n_calcOmega_nb(double time, const Eigen::Vector3d& rollPitchYaw, const Eigen::Quaterniond& n_Quat_b) const
1774{
1775 const auto& R = rollPitchYaw(0);
1776 const auto& P = rollPitchYaw(1);
1777
1778 // #########################################################################################################################################
1779
1780 auto R_dot = static_cast<double>(_splines.roll.derivative(1, time));
1781 auto Y_dot = static_cast<double>(_splines.yaw.derivative(1, time));
1782 auto P_dot = static_cast<double>(_splines.pitch.derivative(1, time));
1783
1784 auto C_3 = [](auto R) {
1785 // Eigen::Matrix3d C;
1786 // C << 1, 0 , 0 ,
1787 // 0, std::cos(R), std::sin(R),
1788 // 0, -std::sin(R), std::cos(R);
1789 // return C;
1790 return Eigen::AngleAxisd{ -R, Eigen::Vector3d::UnitX() };
1791 };
1792 auto C_2 = [](auto P) {
1793 // Eigen::Matrix3d C;
1794 // C << std::cos(P), 0 , -std::sin(P),
1795 // 0 , 1 , 0 ,
1796 // std::sin(P), 0 , std::cos(P);
1797 // return C;
1798 return Eigen::AngleAxisd{ -P, Eigen::Vector3d::UnitY() };
1799 };
1800
1801 // ω_nb_b = [∂/∂t R] + C_3 [ 0 ] + C_3 C_2 [ 0 ]
1802 // [ 0 ] [∂/∂t P] [ 0 ]
1803 // [ 0 ] [ 0 ] [∂/∂t Y]
1804 Eigen::Vector3d b_omega_nb = Eigen::Vector3d{ R_dot, 0, 0 }
1805 + C_3(R) * Eigen::Vector3d{ 0, P_dot, 0 }
1806 + C_3(R) * C_2(P) * Eigen::Vector3d{ 0, 0, Y_dot };
1807
1808 return n_Quat_b * b_omega_nb;
1809}
1810
1811const char* NAV::ImuSimulator::to_string(TrajectoryType value)
1812{
1813 switch (value)
1814 {
1815 case TrajectoryType::Fixed:
1816 return "Fixed";
1817 case TrajectoryType::Linear:
1818 return "Linear";
1819 case TrajectoryType::Circular:
1820 return "Circular";
1821 case TrajectoryType::Csv:
1822 return "CSV";
1823 case TrajectoryType::RoseFigure:
1824 return "Rose Figure";
1825 case TrajectoryType::COUNT:
1826 return "";
1827 }
1828 return "";
1829}
1830
1831const char* NAV::ImuSimulator::to_string(Direction value)
1832{
1833 switch (value)
1834 {
1835 case Direction::CW:
1836 return "Clockwise (CW)";
1837 case Direction::CCW:
1838 return "Counterclockwise (CCW)";
1839 case Direction::COUNT:
1840 return "";
1841 }
1842 return "";
1843}
Ellipsoid.hpp
Functions concerning the ellipsoid model.
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:23
Gravity.hpp
Different Gravity Models.
HelpMarker.hpp
Text Help Marker (?) with Tooltip.
Functions.hpp
Inertial Navigation Helper Functions.
ImuObsSimulated.hpp
Data storage class for simulated IMU observations.
ImuSimulator.hpp
Imu Observation Simulator.
InsTime.hpp
The class is responsible for all time-related tasks.
Mechanization.hpp
Inertial Navigation Mechanization Functions in local navigation frame.
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_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
Math.hpp
Simple Math functions.
PosVelAtt.hpp
Position, Velocity and Attitude Storage Class.
NAV::CsvData::CsvLine
std::vector< CsvElement > CsvLine
CSV Line with splitted entries.
Definition CsvData.hpp:34
NAV::CsvData::type
static std::string type()
Returns the type of the data class.
Definition CsvData.hpp:28
NAV::ImuObsSimulated::type
static std::string type()
Returns the type of the data class.
Definition ImuObsSimulated.hpp:31
NAV::ImuSimulator::_startPosition
gui::widgets::PositionWithFrame _startPosition
Definition ImuSimulator.hpp:152
NAV::ImuSimulator::_roseSimDuration
double _roseSimDuration
Simulation duration needed for the rose figure.
Definition ImuSimulator.hpp:203
NAV::ImuSimulator::pitch
CubicSpline< long double > pitch
Pitch angle [rad].
Definition ImuSimulator.hpp:276
NAV::ImuSimulator::restore
void restore(const json &j) override
Restores the node from a json object.
Definition ImuSimulator.cpp:696
NAV::ImuSimulator::ImuSimulator
ImuSimulator()
Default constructor.
Definition ImuSimulator.cpp:34
NAV::ImuSimulator::deinitialize
void deinitialize() override
Deinitialize the node.
Definition ImuSimulator.cpp:1449
NAV::ImuSimulator::TrajectoryType
TrajectoryType
Types of Trajectories available for simulation.
Definition ImuSimulator.hpp:132
NAV::ImuSimulator::TrajectoryType::Linear
@ Linear
Linear movement with constant velocity.
Definition ImuSimulator.hpp:134
NAV::ImuSimulator::TrajectoryType::Fixed
@ Fixed
Static position without movement.
Definition ImuSimulator.hpp:133
NAV::ImuSimulator::TrajectoryType::COUNT
@ COUNT
Amount of items in the enum.
Definition ImuSimulator.hpp:138
NAV::ImuSimulator::TrajectoryType::Csv
@ Csv
Get the input from the CsvData pin.
Definition ImuSimulator.hpp:136
NAV::ImuSimulator::TrajectoryType::RoseFigure
@ RoseFigure
Movement along a mathmatical rose figure.
Definition ImuSimulator.hpp:137
NAV::ImuSimulator::pollPosVelAtt
std::shared_ptr< const NodeData > pollPosVelAtt(size_t pinIdx, bool peek)
Polls the next simulated data.
Definition ImuSimulator.cpp:1683
NAV::ImuSimulator::_imuInternalUpdateCnt
uint64_t _imuInternalUpdateCnt
Counter to calculate the internal IMU update time.
Definition ImuSimulator.hpp:285
NAV::ImuSimulator::_trajectoryRotationAngle
double _trajectoryRotationAngle
In the GUI selected origin angle of the circular trajectory in [rad].
Definition ImuSimulator.hpp:182
NAV::ImuSimulator::yaw
CubicSpline< long double > yaw
Yaw angle [rad].
Definition ImuSimulator.hpp:277
NAV::ImuSimulator::roll
CubicSpline< long double > roll
Roll angle [rad].
Definition ImuSimulator.hpp:275
NAV::ImuSimulator::StartTimeSource::CurrentComputerTime
@ CurrentComputerTime
Gets the current computer time as start time.
Definition ImuSimulator.hpp:107
NAV::ImuSimulator::StartTimeSource::CustomTime
@ CustomTime
Custom time selected by the user.
Definition ImuSimulator.hpp:106
NAV::ImuSimulator::_lla_imuLastLinearPosition
Eigen::Vector3d _lla_imuLastLinearPosition
Last calculated position for the IMU in linear mode for iterative calculations as latitude,...
Definition ImuSimulator.hpp:299
NAV::ImuSimulator::_circularHarmonicAmplitudeFactor
double _circularHarmonicAmplitudeFactor
Harmonic Oscillation Amplitude Factor of the circle radius [-].
Definition ImuSimulator.hpp:164
NAV::ImuSimulator::OUTPUT_PORT_INDEX_POS_VEL_ATT
static constexpr size_t OUTPUT_PORT_INDEX_POS_VEL_ATT
Flow (PosVelAtt)
Definition ImuSimulator.hpp:75
NAV::ImuSimulator::_trajectoryDirection
Direction _trajectoryDirection
In the GUI selected direction of the circular trajectory (used by circular and rose figure)
Definition ImuSimulator.hpp:179
NAV::ImuSimulator::z
CubicSpline< long double > z
ECEF Z Position [m].
Definition ImuSimulator.hpp:274
NAV::ImuSimulator::_imuInternalFrequency
double _imuInternalFrequency
Frequency to calculate the delta IMU values in [Hz].
Definition ImuSimulator.hpp:122
NAV::ImuSimulator::_csvDuration
double _csvDuration
Duration from the CSV file in [s].
Definition ImuSimulator.hpp:221
NAV::ImuSimulator::_gnssLastUpdateTime
double _gnssLastUpdateTime
Last time the GNSS message was calculated in [s].
Definition ImuSimulator.hpp:297
NAV::ImuSimulator::save
json save() const override
Saves the node into a json object.
Definition ImuSimulator.cpp:649
NAV::ImuSimulator::~ImuSimulator
~ImuSimulator() override
Destructor.
Definition ImuSimulator.cpp:46
NAV::ImuSimulator::n_getAttitudeQuaternionFromCsvLine_b
std::optional< Eigen::Quaterniond > n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine &line, const std::vector< std::string > &description) const
Get the Attitude quaternion n_quat_b from a CSV line.
Definition ImuSimulator.cpp:926
NAV::ImuSimulator::_circularHarmonicFrequency
int _circularHarmonicFrequency
Harmonic Oscillation Frequency on the circular trajectory [cycles/revolution].
Definition ImuSimulator.hpp:161
NAV::ImuSimulator::_rosePetNum
int _rosePetNum
In the GUI selected numerator of petals (2*k for even k, k for uneven k) of the rose figure.
Definition ImuSimulator.hpp:194
NAV::ImuSimulator::OUTPUT_PORT_INDEX_IMU_OBS
static constexpr size_t OUTPUT_PORT_INDEX_IMU_OBS
Flow (ImuObs)
Definition ImuSimulator.hpp:74
NAV::ImuSimulator::_startTimeEditFormat
gui::widgets::TimeEditFormat _startTimeEditFormat
Time Format to input the start time with.
Definition ImuSimulator.hpp:114
NAV::ImuSimulator::_startTime
InsTime _startTime
Global starttime.
Definition ImuSimulator.hpp:117
NAV::ImuSimulator::_linearTrajectoryDistanceForStop
double _linearTrajectoryDistanceForStop
Distance in [m] to the start position to stop the simulation.
Definition ImuSimulator.hpp:224
NAV::ImuSimulator::category
static std::string category()
String representation of the Class Category.
Definition ImuSimulator.cpp:61
NAV::ImuSimulator::n_calcVelocity
Eigen::Vector3d n_calcVelocity(double time, const Eigen::Quaterniond &n_Quat_e) const
Calculates the velocity in local-navigation frame coordinates at the given time depending on the traj...
Definition ImuSimulator.cpp:1741
NAV::ImuSimulator::e_getPositionFromCsvLine
std::optional< Eigen::Vector3d > e_getPositionFromCsvLine(const CsvData::CsvLine &line, const std::vector< std::string > &description) const
Get the Position from a CSV line.
Definition ImuSimulator.cpp:890
NAV::ImuSimulator::_angularRateEarthRotationEnabled
bool _angularRateEarthRotationEnabled
Apply the Earth rotation rate to the measured angular rates.
Definition ImuSimulator.hpp:243
NAV::ImuSimulator::_imuLastUpdateTime
double _imuLastUpdateTime
Last time the IMU message was calculated in [s].
Definition ImuSimulator.hpp:295
NAV::ImuSimulator::_trajectoryVerticalSpeed
double _trajectoryVerticalSpeed
Vertical speed of the vehicle in the tangential plane in [m/s] (used by circular and rose figure)
Definition ImuSimulator.hpp:188
NAV::ImuSimulator::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition ImuSimulator.cpp:51
NAV::ImuSimulator::_rosePetDenom
int _rosePetDenom
In the GUI selected denominator of petals (2*k for even k, k for uneven k) of the rose figure.
Definition ImuSimulator.hpp:197
NAV::ImuSimulator::INPUT_PORT_INDEX_CSV
static constexpr size_t INPUT_PORT_INDEX_CSV
Object (CsvData)
Definition ImuSimulator.hpp:73
NAV::ImuSimulator::_trajectoryType
TrajectoryType _trajectoryType
Selected trajectory type in the GUI.
Definition ImuSimulator.hpp:146
NAV::ImuSimulator::Direction
Direction
Possible directions for the circular trajectory.
Definition ImuSimulator.hpp:168
NAV::ImuSimulator::Direction::COUNT
@ COUNT
Amount of items in the enum.
Definition ImuSimulator.hpp:171
NAV::ImuSimulator::_roseTrajectoryCountForStop
double _roseTrajectoryCountForStop
Amount of rose figures to simulate before stopping.
Definition ImuSimulator.hpp:230
NAV::ImuSimulator::_roseStepLengthMax
double _roseStepLengthMax
Maxmimum step length for the spline points for the rose figure [m]. Points will be spaced between [L/...
Definition ImuSimulator.hpp:200
NAV::ImuSimulator::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition ImuSimulator.cpp:66
NAV::ImuSimulator::y
CubicSpline< long double > y
ECEF Y Position [m].
Definition ImuSimulator.hpp:273
NAV::ImuSimulator::_centrifgalAccelerationEnabled
bool _centrifgalAccelerationEnabled
Apply the centrifugal acceleration to the measured accelerations.
Definition ImuSimulator.hpp:240
NAV::ImuSimulator::_simulationDuration
double _simulationDuration
Duration to simulate in [s].
Definition ImuSimulator.hpp:218
NAV::ImuSimulator::resetNode
bool resetNode() override
Resets the node. Moves the read cursor to the start.
Definition ImuSimulator.cpp:1454
NAV::ImuSimulator::DistanceOrCirclesOrRoses
@ DistanceOrCirclesOrRoses
Distance for Linear trajectory / Circle count for Circular / Count for rose figure trajectory.
Definition ImuSimulator.hpp:211
NAV::ImuSimulator::Duration
@ Duration
Time Duration.
Definition ImuSimulator.hpp:210
NAV::ImuSimulator::calcFlightAngles
std::array< double, 3 > calcFlightAngles(double time) const
Calculates the flight angles (roll, pitch, yaw)
Definition ImuSimulator.cpp:1724
NAV::ImuSimulator::initializeSplines
bool initializeSplines()
Initializes the spline values.
Definition ImuSimulator.cpp:964
NAV::ImuSimulator::_circularTrajectoryCircleCountForStop
double _circularTrajectoryCircleCountForStop
Amount of circles to simulate before stopping.
Definition ImuSimulator.hpp:227
NAV::ImuSimulator::pollImuObs
std::shared_ptr< const NodeData > pollImuObs(size_t pinIdx, bool peek)
Polls the next simulated data.
Definition ImuSimulator.cpp:1535
NAV::ImuSimulator::_n_linearTrajectoryStartVelocity
Eigen::Vector3d _n_linearTrajectoryStartVelocity
Start Velocity of the vehicle in local-navigation frame cooridnates in [m/s].
Definition ImuSimulator.hpp:158
NAV::ImuSimulator::_imuUpdateCnt
uint64_t _imuUpdateCnt
Counter to calculate the IMU update time.
Definition ImuSimulator.hpp:287
NAV::ImuSimulator::n_calcOmega_nb
Eigen::Vector3d n_calcOmega_nb(double time, const Eigen::Vector3d &rollPitchYaw, const Eigen::Quaterniond &n_Quat_b) const
Calculates ω_nb_n, the turn rate of the body with respect to the navigation system expressed in NED c...
Definition ImuSimulator.cpp:1773
NAV::ImuSimulator::lla_calcPosition
Eigen::Vector3d lla_calcPosition(double time) const
Calculates the position in latLonAlt at the given time depending on the trajectoryType.
Definition ImuSimulator.cpp:1733
NAV::ImuSimulator::_angularRateTransportRateEnabled
bool _angularRateTransportRateEnabled
Apply the transport rate to the measured angular rates.
Definition ImuSimulator.hpp:246
NAV::ImuSimulator::to_string
static const char * to_string(TrajectoryType value)
Converts the enum to a string.
Definition ImuSimulator.cpp:1811
NAV::ImuSimulator::type
std::string type() const override
String representation of the Class Type.
Definition ImuSimulator.cpp:56
NAV::ImuSimulator::_splines
struct NAV::ImuSimulator::@112323040341172005327360071040367216370163224314 _splines
Assign a variable that holds the Spline information.
NAV::ImuSimulator::_gravitationModel
GravitationModel _gravitationModel
Gravitation model selected in the GUI.
Definition ImuSimulator.hpp:234
NAV::ImuSimulator::checkStopCondition
bool checkStopCondition(double time, const Eigen::Vector3d &lla_position)
Checks the selected stop condition.
Definition ImuSimulator.cpp:1501
NAV::ImuSimulator::_gnssUpdateCnt
uint64_t _gnssUpdateCnt
Counter to calculate the GNSS update time.
Definition ImuSimulator.hpp:289
NAV::ImuSimulator::_lla_gnssLastLinearPosition
Eigen::Vector3d _lla_gnssLastLinearPosition
Last calculated position for the GNSS in linear mode for iterative calculations as latitude,...
Definition ImuSimulator.hpp:301
NAV::ImuSimulator::_simulationStopCondition
StopCondition _simulationStopCondition
Condition which has to be met to stop the simulation.
Definition ImuSimulator.hpp:215
NAV::ImuSimulator::_trajectoryRadius
double _trajectoryRadius
In the GUI selected radius of the circular trajectory (used by circular and rose figure)
Definition ImuSimulator.hpp:191
NAV::ImuSimulator::_startTimeSource
StartTimeSource _startTimeSource
Source for the start time, selected in the GUI.
Definition ImuSimulator.hpp:111
NAV::ImuSimulator::_p_lastImuAccelerationMeas
Eigen::Vector3d _p_lastImuAccelerationMeas
Last calculated acceleration measurement in platform coordinates [m/s²].
Definition ImuSimulator.hpp:303
NAV::ImuSimulator::_trajectoryHorizontalSpeed
double _trajectoryHorizontalSpeed
Horizontal speed of the vehicle in the tangential plane in [m/s] (used by circular and rose figure)
Definition ImuSimulator.hpp:185
NAV::ImuSimulator::_coriolisAccelerationEnabled
bool _coriolisAccelerationEnabled
Apply the coriolis acceleration to the measured accelerations.
Definition ImuSimulator.hpp:237
NAV::ImuSimulator::_p_lastImuAngularRateMeas
Eigen::Vector3d _p_lastImuAngularRateMeas
Last calculated angular rate measurement in platform coordinates [rad/s].
Definition ImuSimulator.hpp:305
NAV::ImuSimulator::n_calcTrajectoryAccel
Eigen::Vector3d n_calcTrajectoryAccel(double time, const Eigen::Quaterniond &n_Quat_e, const Eigen::Vector3d &lla_position, const Eigen::Vector3d &n_velocity) const
Calculates the acceleration in local-navigation frame coordinates at the given time depending on the ...
Definition ImuSimulator.cpp:1749
NAV::ImuSimulator::initialize
bool initialize() override
Initialize the node.
Definition ImuSimulator.cpp:1442
NAV::ImuSimulator::getTimeFromCsvLine
std::optional< InsTime > getTimeFromCsvLine(const CsvData::CsvLine &line, const std::vector< std::string > &description) const
Get the Time from a CSV line.
Definition ImuSimulator.cpp:844
NAV::ImuSimulator::x
CubicSpline< long double > x
ECEF X Position [m].
Definition ImuSimulator.hpp:272
NAV::ImuSimulator::_gnssFrequency
double _gnssFrequency
Frequency to sample the position with in [Hz].
Definition ImuSimulator.hpp:126
NAV::ImuSimulator::_imuFrequency
double _imuFrequency
Frequency to sample the IMU with in [Hz].
Definition ImuSimulator.hpp:124
NAV::ImuSimulator::_fixedTrajectoryStartOrientation
Eigen::Vector3d _fixedTrajectoryStartOrientation
Orientation of the vehicle [roll, pitch, yaw] [rad].
Definition ImuSimulator.hpp:155
NAV::Imu::save
json save() const override
Saves the node into a json object.
Definition Imu.cpp:93
NAV::Imu::restore
void restore(const json &j) override
Restores the node from a json object.
Definition Imu.cpp:104
NAV::Imu::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition Imu.cpp:55
NAV::Imu::Imu
Imu(const Imu &)=delete
Copy constructor.
NAV::Imu::_imuPos
ImuPos _imuPos
Position and rotation information for conversion from platform to body frame.
Definition Imu.hpp:57
NAV::InsConst::e_omega_ie
static const Eigen::Vector3< double > e_omega_ie
ω_ie_e = ω_ie_i Nominal mean angular velocity of the Earth in [rad/s], in earth coordinates
Definition Constants.hpp:222
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
NAV::Node::doDeinitialize
bool doDeinitialize(bool wait=false)
Asks the node worker to deinitialize the node.
Definition Node.cpp:465
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:522
NAV::Node::getInputValue
std::optional< InputPin::IncomingLink::ValueWrapper< T > > getInputValue(size_t portIndex) const
Get Input Value connected on the pin. Only const data types.
Definition Node.hpp:290
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::PosVelAtt::type
static std::string type()
Returns the type of the data class.
Definition PosVelAtt.hpp:29
NAV::gui::NodeEditorApplication::windowFontRatio
static float windowFontRatio()
Ratio to multiply for GUI window elements.
Definition NodeEditorApplication.cpp:1922
NAV::gui::NodeEditorApplication::m_RoseFigure
static ImTextureID m_RoseFigure
Pointer to the texture for the rose figure (ImuSimulator node)
Definition NodeEditorApplication.hpp:111
imgui_ex.hpp
ImGui extensions.
ImGui::InputIntL
bool InputIntL(const char *label, int *v, int v_min, int v_max, int step, int step_fast, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'int'.
Definition imgui_ex.cpp:242
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
ImGui::InputDoubleL
bool InputDoubleL(const char *label, double *v, double v_min, double v_max, double step, double step_fast, const char *format, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'double'.
Definition imgui_ex.cpp:294
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:682
NAV::gui::widgets::BeginHelpMarker
bool BeginHelpMarker(const char *symbol="(?)", float textWrapLength=35.0F)
Begins a Text Help Marker, e.g. '(?)', with custom content.
Definition HelpMarker.cpp:28
NAV::gui::widgets::EndHelpMarker
void EndHelpMarker(bool wrapText=true)
Ends a Text Help Marker with custom content.
Definition HelpMarker.cpp:44
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::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::HelpMarker
void HelpMarker(const char *desc, const char *symbol="(?)")
Text Help Marker, e.g. '(?)', with Tooltip.
Definition HelpMarker.cpp:14
NAV::math::calcEllipticalIntegral
double calcEllipticalIntegral(double phi, double m)
Calculates the incomplete elliptical integral of the second kind.
Definition Math.cpp:53
NAV::math::skewSymmetricMatrix
Eigen::Matrix< typename Derived::Scalar, 3, 3 > skewSymmetricMatrix(const Eigen::MatrixBase< Derived > &a)
Calculates the skew symmetric matrix of the given vector. This is needed to perform the cross product...
Definition Math.hpp:99
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::b_Quat_n
Eigen::Quaternion< Scalar > b_Quat_n(const Scalar &roll, const Scalar &pitch, const Scalar &yaw)
Quaternion for rotations from navigation to body frame.
Definition CoordinateFrames.hpp:335
NAV::trafo::e_Quat_n
Eigen::Quaternion< Scalar > e_Quat_n(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from navigation to Earth-fixed frame.
Definition CoordinateFrames.hpp:309
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::UTC
@ UTC
Coordinated Universal Time.
Definition TimeSystem.hpp:29
NAV::n_calcGravitation
Eigen::Vector3< typename Derived::Scalar > n_calcGravitation(const Eigen::MatrixBase< Derived > &lla_position, GravitationModel gravitationModel=GravitationModel::EGM96)
Calculates the gravitation (acceleration due to mass attraction of the Earth)
Definition Gravity.hpp:202
NAV::e_calcCentrifugalAcceleration
Eigen::Vector3< typename DerivedA::Scalar > e_calcCentrifugalAcceleration(const Eigen::MatrixBase< DerivedA > &e_position, const Eigen::MatrixBase< DerivedB > &e_omega_ie=InsConst::e_omega_ie)
Calculates the centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved pa...
Definition Functions.hpp:72
NAV::n_calcCoriolisAcceleration
Eigen::Vector3< typename DerivedA::Scalar > n_calcCoriolisAcceleration(const Eigen::MatrixBase< DerivedA > &n_omega_ie, const Eigen::MatrixBase< DerivedB > &n_omega_en, const Eigen::MatrixBase< DerivedC > &n_velocity)
Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference fra...
Definition Functions.hpp:90
NAV::calcGeographicalDistance
Scalar calcGeographicalDistance(Scalar lat1, Scalar lon1, Scalar lat2, Scalar lon2)
Measure the distance between two points over an ellipsoidal-surface.
Definition Ellipsoid.hpp:107
NAV::calcEarthRadius_N
Scalar calcEarthRadius_N(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the North/South (meridian) earth radius.
Definition Ellipsoid.hpp:42
NAV::to_string
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Definition PositionInput.cpp:25
NAV::n_calcTransportRate
Eigen::Vector3< typename DerivedA::Scalar > n_calcTransportRate(const Eigen::MatrixBase< DerivedA > &lla_position, const Eigen::MatrixBase< DerivedB > &n_velocity, const auto &R_N, const auto &R_E)
Calculates the transport rate ω_en_n (rotation rate of the Earth frame relative to the navigation fra...
Definition Functions.hpp:39
NAV::ComboGravitationModel
bool ComboGravitationModel(const char *label, GravitationModel &gravitationModel)
Shows a ComboBox to select the gravitation model.
Definition Gravity.cpp:40
NAV::calcPitchFromVelocity
Derived::Scalar calcPitchFromVelocity(const Eigen::MatrixBase< Derived > &n_velocity)
Calculates the Pitch angle from the trajectory defined by the given velocity.
Definition Functions.hpp:169
NAV::deg2rad
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
NAV::calcYawFromVelocity
Derived::Scalar calcYawFromVelocity(const Eigen::MatrixBase< Derived > &n_velocity)
Calculates the Yaw angle from the trajectory defined by the given velocity.
Definition Functions.hpp:153
NAV::lla_calcTimeDerivativeForPosition
Eigen::Vector3< typename Derived::Scalar > lla_calcTimeDerivativeForPosition(const Eigen::MatrixBase< Derived > &n_velocity, const auto &phi, const auto &h, const auto &R_N, const auto &R_E)
Calculates the time derivative of the curvilinear position.
Definition Mechanization.hpp:119
NAV::rad2deg
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39
NAV::calcEarthRadius_E
Scalar calcEarthRadius_E(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the East/West (prime vertical) earth radius.
Definition Ellipsoid.hpp:58
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52
NAV::Pin::Type::Object
@ Object
Generic Object.
Definition Pin.hpp:57