| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | #include "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 | |||
| 34 | 121 | NAV::ImuSimulator::ImuSimulator() | |
| 35 |
17/34✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
✓ Branch 10 taken 121 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 121 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 121 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 121 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 121 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 121 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 121 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 121 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 121 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 121 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 121 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 121 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 121 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 121 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 121 times.
✗ Branch 54 not taken.
|
121 | : Imu(typeStatic()) |
| 36 | { | ||
| 37 | LOG_TRACE("{}: called", name); | ||
| 38 | |||
| 39 | 121 | _hasConfig = true; | |
| 40 | 121 | _guiConfigDefaultWindowSize = { 677, 508 }; | |
| 41 | |||
| 42 |
4/8✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 121 times.
✓ Branch 9 taken 121 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
363 | CreateOutputPin("ImuObs", Pin::Type::Flow, { NAV::ImuObsSimulated::type() }, &ImuSimulator::pollImuObs); |
| 43 |
4/8✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 121 times.
✓ Branch 9 taken 121 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
363 | CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &ImuSimulator::pollPosVelAtt); |
| 44 | 363 | } | |
| 45 | |||
| 46 | 256 | NAV::ImuSimulator::~ImuSimulator() | |
| 47 | { | ||
| 48 | LOG_TRACE("{}: called", nameId()); | ||
| 49 | 256 | } | |
| 50 | |||
| 51 | 235 | std::string NAV::ImuSimulator::typeStatic() | |
| 52 | { | ||
| 53 |
1/2✓ Branch 1 taken 235 times.
✗ Branch 2 not taken.
|
470 | return "ImuSimulator"; |
| 54 | } | ||
| 55 | |||
| 56 | ✗ | std::string NAV::ImuSimulator::type() const | |
| 57 | { | ||
| 58 | ✗ | return typeStatic(); | |
| 59 | } | ||
| 60 | |||
| 61 | 114 | std::string NAV::ImuSimulator::category() | |
| 62 | { | ||
| 63 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
228 | return "Data Simulator"; |
| 64 | } | ||
| 65 | |||
| 66 | ✗ | void 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 | |||
| 649 | ✗ | json 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 | |||
| 696 | 7 | void NAV::ImuSimulator::restore(json const& j) | |
| 697 | { | ||
| 698 | LOG_TRACE("{}: called", nameId()); | ||
| 699 | |||
| 700 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("startTimeSource")) |
| 701 | { | ||
| 702 | 7 | j.at("startTimeSource").get_to(_startTimeSource); | |
| 703 | } | ||
| 704 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("startTimeEditFormat")) |
| 705 | { | ||
| 706 | 7 | j.at("startTimeEditFormat").get_to(_startTimeEditFormat); | |
| 707 | } | ||
| 708 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("startTime")) |
| 709 | { | ||
| 710 | 7 | j.at("startTime").get_to(_startTime); | |
| 711 | } | ||
| 712 | // ########################################################################################################### | ||
| 713 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("imuInternalFrequency")) |
| 714 | { | ||
| 715 | 7 | j.at("imuInternalFrequency").get_to(_imuInternalFrequency); | |
| 716 | } | ||
| 717 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("imuFrequency")) |
| 718 | { | ||
| 719 | 7 | j.at("imuFrequency").get_to(_imuFrequency); | |
| 720 | } | ||
| 721 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("gnssFrequency")) |
| 722 | { | ||
| 723 | 7 | j.at("gnssFrequency").get_to(_gnssFrequency); | |
| 724 | } | ||
| 725 | // ########################################################################################################### | ||
| 726 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("trajectoryType")) |
| 727 | { | ||
| 728 | 7 | j.at("trajectoryType").get_to(_trajectoryType); | |
| 729 | |||
| 730 |
2/6✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
|
7 | if (_trajectoryType == TrajectoryType::Csv && inputPins.empty()) |
| 731 | { | ||
| 732 | ✗ | CreateInputPin(CsvData::type().c_str(), Pin::Type::Object, { CsvData::type() }); | |
| 733 | } | ||
| 734 |
3/6✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
|
7 | else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty()) |
| 735 | { | ||
| 736 | ✗ | DeleteInputPin(0); | |
| 737 | } | ||
| 738 | } | ||
| 739 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("startPosition")) |
| 740 | { | ||
| 741 | 7 | j.at("startPosition").get_to(_startPosition); | |
| 742 | } | ||
| 743 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("fixedTrajectoryStartOrientation")) |
| 744 | { | ||
| 745 | 7 | j.at("fixedTrajectoryStartOrientation").get_to(_fixedTrajectoryStartOrientation); | |
| 746 | } | ||
| 747 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("n_linearTrajectoryStartVelocity")) |
| 748 | { | ||
| 749 | 7 | j.at("n_linearTrajectoryStartVelocity").get_to(_n_linearTrajectoryStartVelocity); | |
| 750 | } | ||
| 751 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("circularHarmonicFrequency")) |
| 752 | { | ||
| 753 | 7 | j.at("circularHarmonicFrequency").get_to(_circularHarmonicFrequency); | |
| 754 | } | ||
| 755 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("circularHarmonicAmplitudeFactor")) |
| 756 | { | ||
| 757 | 7 | j.at("circularHarmonicAmplitudeFactor").get_to(_circularHarmonicAmplitudeFactor); | |
| 758 | } | ||
| 759 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("trajectoryHorizontalSpeed")) |
| 760 | { | ||
| 761 | 7 | j.at("trajectoryHorizontalSpeed").get_to(_trajectoryHorizontalSpeed); | |
| 762 | } | ||
| 763 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("trajectoryVerticalSpeed")) |
| 764 | { | ||
| 765 | 7 | j.at("trajectoryVerticalSpeed").get_to(_trajectoryVerticalSpeed); | |
| 766 | } | ||
| 767 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("trajectoryRadius")) |
| 768 | { | ||
| 769 | 7 | j.at("trajectoryRadius").get_to(_trajectoryRadius); | |
| 770 | } | ||
| 771 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("trajectoryRotationAngle")) |
| 772 | { | ||
| 773 | 7 | j.at("trajectoryRotationAngle").get_to(_trajectoryRotationAngle); | |
| 774 | } | ||
| 775 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("trajectoryDirection")) |
| 776 | { | ||
| 777 | 7 | j.at("trajectoryDirection").get_to(_trajectoryDirection); | |
| 778 | } | ||
| 779 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("rosePetNum")) |
| 780 | { | ||
| 781 | 7 | j.at("rosePetNum").get_to(_rosePetNum); | |
| 782 | } | ||
| 783 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("rosePetDenom")) |
| 784 | { | ||
| 785 | 7 | j.at("rosePetDenom").get_to(_rosePetDenom); | |
| 786 | } | ||
| 787 | // ########################################################################################################### | ||
| 788 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("simulationStopCondition")) |
| 789 | { | ||
| 790 | 7 | j.at("simulationStopCondition").get_to(_simulationStopCondition); | |
| 791 | } | ||
| 792 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("simulationDuration")) |
| 793 | { | ||
| 794 | 7 | j.at("simulationDuration").get_to(_simulationDuration); | |
| 795 | } | ||
| 796 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("linearTrajectoryDistanceForStop")) |
| 797 | { | ||
| 798 | 7 | j.at("linearTrajectoryDistanceForStop").get_to(_linearTrajectoryDistanceForStop); | |
| 799 | } | ||
| 800 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("circularTrajectoryCircleCountForStop")) |
| 801 | { | ||
| 802 | 7 | j.at("circularTrajectoryCircleCountForStop").get_to(_circularTrajectoryCircleCountForStop); | |
| 803 | } | ||
| 804 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("roseTrajectoryCountForStop")) |
| 805 | { | ||
| 806 | 7 | j.at("roseTrajectoryCountForStop").get_to(_roseTrajectoryCountForStop); | |
| 807 | } | ||
| 808 | // ########################################################################################################### | ||
| 809 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("splineSampleInterval")) |
| 810 | { | ||
| 811 | 7 | j.at("splineSampleInterval").get_to(_splines.sampleInterval); | |
| 812 | } | ||
| 813 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("roseStepLengthMax")) |
| 814 | { | ||
| 815 | 7 | j.at("roseStepLengthMax").get_to(_roseStepLengthMax); | |
| 816 | } | ||
| 817 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("gravitationModel")) |
| 818 | { | ||
| 819 | 7 | j.at("gravitationModel").get_to(_gravitationModel); | |
| 820 | } | ||
| 821 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("coriolisAccelerationEnabled")) |
| 822 | { | ||
| 823 | 7 | j.at("coriolisAccelerationEnabled").get_to(_coriolisAccelerationEnabled); | |
| 824 | } | ||
| 825 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("centrifgalAccelerationEnabled")) |
| 826 | { | ||
| 827 | 7 | j.at("centrifgalAccelerationEnabled").get_to(_centrifgalAccelerationEnabled); | |
| 828 | } | ||
| 829 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("angularRateEarthRotationEnabled")) |
| 830 | { | ||
| 831 | 7 | j.at("angularRateEarthRotationEnabled").get_to(_angularRateEarthRotationEnabled); | |
| 832 | } | ||
| 833 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("angularRateTransportRateEnabled")) |
| 834 | { | ||
| 835 | 7 | j.at("angularRateTransportRateEnabled").get_to(_angularRateTransportRateEnabled); | |
| 836 | } | ||
| 837 | // ########################################################################################################### | ||
| 838 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("Imu")) |
| 839 | { | ||
| 840 | 7 | Imu::restore(j.at("Imu")); | |
| 841 | } | ||
| 842 | 7 | } | |
| 843 | |||
| 844 | ✗ | std::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 | |||
| 890 | ✗ | std::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 | |||
| 926 | ✗ | std::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 | |||
| 964 | 7 | bool NAV::ImuSimulator::initializeSplines() | |
| 965 | { | ||
| 966 | 7 | std::vector<long double> splineTime; | |
| 967 | |||
| 968 | 1343744 | auto unwrapAngle = [](auto angle, auto prevAngle, auto rangeMax) { | |
| 969 | 1343744 | auto x = angle - prevAngle; | |
| 970 | 1343744 | x = std::fmod(x + rangeMax, 2 * rangeMax); | |
| 971 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1343744 times.
|
1343744 | if (x < 0) |
| 972 | { | ||
| 973 | ✗ | x += 2 * rangeMax; | |
| 974 | } | ||
| 975 | 1343744 | x -= rangeMax; | |
| 976 | |||
| 977 | 1343744 | return prevAngle + x; | |
| 978 | }; | ||
| 979 | |||
| 980 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
7 | if (_trajectoryType == TrajectoryType::Fixed) |
| 981 | { | ||
| 982 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(-30.0); // 10 seconds in the past; simply to define the derivative at zero seconds |
| 983 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(-20.0); // 10 seconds in the past; simply to define the derivative at zero seconds |
| 984 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(-10.0); // 10 seconds in the past; simply to define the derivative at zero seconds |
| 985 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(0.0); |
| 986 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(_simulationDuration); |
| 987 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(_simulationDuration + 10.0); // 10 seconds past simulation end; simply to define the derivative at end node |
| 988 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(_simulationDuration + 20.0); // 10 seconds past simulation end; simply to define the derivative at end node |
| 989 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(_simulationDuration + 30.0); // 10 seconds past simulation end; simply to define the derivative at end node |
| 990 | |||
| 991 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | Eigen::Vector3d e_startPosition = _startPosition.e_position.cast<double>(); |
| 992 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | std::vector<long double> X(splineTime.size(), e_startPosition[0]); |
| 993 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | std::vector<long double> Y(splineTime.size(), e_startPosition[1]); |
| 994 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | std::vector<long double> Z(splineTime.size(), e_startPosition[2]); |
| 995 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | std::vector<long double> Roll(splineTime.size(), _fixedTrajectoryStartOrientation.x()); |
| 996 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | std::vector<long double> Pitch(splineTime.size(), _fixedTrajectoryStartOrientation.y()); |
| 997 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | std::vector<long double> Yaw(splineTime.size(), _fixedTrajectoryStartOrientation.z()); |
| 998 | |||
| 999 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.x.setPoints(splineTime, X); |
| 1000 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.y.setPoints(splineTime, Y); |
| 1001 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.z.setPoints(splineTime, Z); |
| 1002 | |||
| 1003 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.roll.setPoints(splineTime, Roll); |
| 1004 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.pitch.setPoints(splineTime, Pitch); |
| 1005 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.yaw.setPoints(splineTime, Yaw); |
| 1006 | 6 | } | |
| 1007 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | 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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | 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 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | 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 | 1 | int n = _rosePetNum; | |
| 1178 | 1 | int d = _rosePetDenom; | |
| 1179 | |||
| 1180 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | for (int i = 2; i <= n; i++) // reduction of fraction ( 4/2 ==> 2/1 ) |
| 1181 | { | ||
| 1182 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
|
1 | if (n % i == 0 && d % i == 0) |
| 1183 | { | ||
| 1184 | ✗ | n /= i; | |
| 1185 | ✗ | d /= i; | |
| 1186 | ✗ | i--; | |
| 1187 | } | ||
| 1188 | } | ||
| 1189 | |||
| 1190 | 2 | 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 | 1 | double integrationFactor = 0.0; | |
| 1194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (isOdd(d)) |
| 1195 | { | ||
| 1196 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (isOdd(n)) { integrationFactor = static_cast<double>(d); } |
| 1197 | 1 | 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 | 1 | auto nVirtPoints = static_cast<size_t>(1.0 / (_roseStepLengthMax / 50.0)); | |
| 1206 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | splineTime.resize(nVirtPoints); // Preallocate points to make the spline start at the right point |
| 1207 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineX(splineTime.size()); |
| 1208 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineY(splineTime.size()); |
| 1209 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::vector<long double> splineZ(splineTime.size()); |
| 1210 | |||
| 1211 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Eigen::Vector3d e_origin = trafo::lla2ecef_WGS84(_startPosition.latLonAlt()); |
| 1212 | |||
| 1213 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
1 | Eigen::Quaterniond e_quatCenter_n = trafo::e_Quat_n(_startPosition.latLonAlt()(0), _startPosition.latLonAlt()(1)); |
| 1214 | |||
| 1215 | 1 | double lengthOld = -_roseStepLengthMax / 2.0; // n-1 length | |
| 1216 | 1 | double dPhi = 0.001; // Angle step size | |
| 1217 | 1 | double maxPhi = std::numeric_limits<double>::infinity(); // Interval for integration depending on selected stop criteria | |
| 1218 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses) |
| 1219 | { | ||
| 1220 | 1 | maxPhi = _roseTrajectoryCountForStop * integrationFactor * M_PI; | |
| 1221 | } | ||
| 1222 | |||
| 1223 | // k = n/d | ||
| 1224 | 1 | double roseK = static_cast<double>(_rosePetNum) / static_cast<double>(_rosePetDenom); | |
| 1225 | |||
| 1226 | 1 | _roseSimDuration = 0.0; | |
| 1227 | |||
| 1228 | // We cannot input negative values or zero | ||
| 1229 |
2/2✓ Branch 0 taken 1342086 times.
✓ Branch 1 taken 1 times.
|
1342087 | for (double phi = dPhi; phi <= maxPhi + static_cast<double>(nVirtPoints) * dPhi; phi += dPhi) // NOLINT(clang-analyzer-security.FloatLoopCounter, cert-flp30-c) |
| 1230 | { | ||
| 1231 |
1/2✓ Branch 1 taken 1342086 times.
✗ Branch 2 not taken.
|
1342086 | double length = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * phi, 1.0 - std::pow(roseK, 2.0)); |
| 1232 | 1342086 | double dL = length - lengthOld; | |
| 1233 | |||
| 1234 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1342080 times.
|
1342086 | if (dL > _roseStepLengthMax) |
| 1235 | { | ||
| 1236 | 6 | phi -= dPhi; | |
| 1237 | 6 | dPhi /= 2.0; | |
| 1238 | 7 | continue; | |
| 1239 | } | ||
| 1240 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1342079 times.
|
1342080 | if (dL < _roseStepLengthMax / 3.0) // Avoid also too small steps |
| 1241 | { | ||
| 1242 | 1 | phi -= dPhi; | |
| 1243 | 1 | dPhi *= 1.5; | |
| 1244 | 1 | continue; | |
| 1245 | } | ||
| 1246 | 1342079 | lengthOld = length; | |
| 1247 | |||
| 1248 | 1342079 | double time = length / _trajectoryHorizontalSpeed; | |
| 1249 |
1/2✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
|
1342079 | splineTime.push_back(time); |
| 1250 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1342079 times.
|
1342079 | Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::cos(roseK * phi) * std::sin(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m] |
| 1251 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1342079 times.
|
1342079 | _trajectoryRadius * std::cos(roseK * phi) * std::cos(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m] |
| 1252 |
1/2✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
|
1342079 | -_trajectoryVerticalSpeed * time }; // [m] |
| 1253 | |||
| 1254 |
1/2✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
|
1342079 | Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition; |
| 1255 |
2/4✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1342079 times.
✗ Branch 5 not taken.
|
1342079 | 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 |
2/4✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1342079 times.
✗ Branch 5 not taken.
|
1342079 | splineX.push_back(e_position[0]); |
| 1260 |
2/4✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1342079 times.
✗ Branch 5 not taken.
|
1342079 | splineY.push_back(e_position[1]); |
| 1261 |
2/4✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1342079 times.
✗ Branch 5 not taken.
|
1342079 | splineZ.push_back(e_position[2]); |
| 1262 | |||
| 1263 |
5/6✓ Branch 0 taken 1342079 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 1342077 times.
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 1342077 times.
|
1342079 | 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 | 2 | _roseSimDuration = time; | |
| 1267 | } | ||
| 1268 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 1342077 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
1342077 | else if (_simulationStopCondition == StopCondition::Duration && _roseSimDuration == 0.0 && time > _simulationDuration) |
| 1269 | { | ||
| 1270 | ✗ | _roseSimDuration = _simulationDuration; | |
| 1271 | ✗ | maxPhi = phi; | |
| 1272 | } | ||
| 1273 | } | ||
| 1274 | |||
| 1275 | 1 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double endLength = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * maxPhi, 1.0 - std::pow(roseK, 2.0)); |
| 1278 | // LOG_DATA("endLength {:8.6}m", endLength); | ||
| 1279 |
2/2✓ Branch 0 taken 1666 times.
✓ Branch 1 taken 1 times.
|
1667 | for (size_t i = 0; i < nVirtPoints; i++) |
| 1280 | { | ||
| 1281 | 1666 | double phi = maxPhi - static_cast<double>(i) * dPhi; | |
| 1282 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | double length = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * phi, 1.0 - std::pow(roseK, 2.0)); |
| 1283 | 1666 | double time = (length - endLength) / _trajectoryHorizontalSpeed; | |
| 1284 | 1666 | splineTime[nVirtPoints - i - 1] = time; | |
| 1285 | |||
| 1286 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1666 times.
|
1666 | Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::cos(roseK * phi) * std::sin(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m] |
| 1287 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1666 times.
|
1666 | _trajectoryRadius * std::cos(roseK * phi) * std::cos(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m] |
| 1288 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | -_trajectoryVerticalSpeed * time }; // [m] |
| 1289 | |||
| 1290 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition; |
| 1291 |
2/4✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1666 times.
✗ Branch 5 not taken.
|
1666 | 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 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | splineX[nVirtPoints - i - 1] = e_position[0]; |
| 1296 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | splineY[nVirtPoints - i - 1] = e_position[1]; |
| 1297 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | splineZ[nVirtPoints - i - 1] = e_position[2]; |
| 1298 | } | ||
| 1299 | |||
| 1300 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.x.setPoints(splineTime, splineX); |
| 1301 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.y.setPoints(splineTime, splineY); |
| 1302 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.z.setPoints(splineTime, splineZ); |
| 1303 | 1 | } | |
| 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 |
3/4✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 6 times.
|
7 | if (_trajectoryType == TrajectoryType::Circular || _trajectoryType == TrajectoryType::RoseFigure) |
| 1401 | { | ||
| 1402 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineRoll(splineTime.size()); |
| 1403 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splinePitch(splineTime.size()); |
| 1404 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::vector<long double> splineYaw(splineTime.size()); |
| 1405 | |||
| 1406 |
2/2✓ Branch 1 taken 1343745 times.
✓ Branch 2 taken 1 times.
|
1343746 | for (uint64_t i = 0; i < splineTime.size(); i++) |
| 1407 | { | ||
| 1408 |
1/2✓ Branch 2 taken 1343745 times.
✗ Branch 3 not taken.
|
1343745 | Eigen::Vector3d e_pos{ static_cast<double>(_splines.x(splineTime[i])), |
| 1409 |
1/2✓ Branch 2 taken 1343745 times.
✗ Branch 3 not taken.
|
1343745 | static_cast<double>(_splines.y(splineTime[i])), |
| 1410 |
2/4✓ Branch 2 taken 1343745 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1343745 times.
✗ Branch 6 not taken.
|
1343745 | static_cast<double>(_splines.z(splineTime[i])) }; |
| 1411 |
1/2✓ Branch 2 taken 1343745 times.
✗ Branch 3 not taken.
|
1343745 | Eigen::Vector3d e_vel{ static_cast<double>(_splines.x.derivative(1, splineTime[i])), |
| 1412 |
1/2✓ Branch 2 taken 1343745 times.
✗ Branch 3 not taken.
|
1343745 | static_cast<double>(_splines.y.derivative(1, splineTime[i])), |
| 1413 |
2/4✓ Branch 2 taken 1343745 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1343745 times.
✗ Branch 6 not taken.
|
1343745 | static_cast<double>(_splines.z.derivative(1, splineTime[i])) }; |
| 1414 | |||
| 1415 |
1/2✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
|
1343745 | Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(e_pos); |
| 1416 |
4/8✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1343745 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1343745 times.
✗ Branch 11 not taken.
|
1343745 | Eigen::Vector3d n_velocity = trafo::n_Quat_e(lla_position(0), lla_position(1)) * e_vel; |
| 1417 | |||
| 1418 |
4/8✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1343745 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1343745 times.
✗ Branch 11 not taken.
|
1343745 | Eigen::Vector3d e_normalVectorCenterCircle{ std::cos(_startPosition.latLonAlt()(0)) * std::cos(_startPosition.latLonAlt()(1)), |
| 1419 |
4/8✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1343745 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1343745 times.
✗ Branch 11 not taken.
|
1343745 | std::cos(_startPosition.latLonAlt()(0)) * std::sin(_startPosition.latLonAlt()(1)), |
| 1420 |
3/6✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1343745 times.
✗ Branch 8 not taken.
|
1343745 | std::sin(_startPosition.latLonAlt()(0)) }; |
| 1421 | |||
| 1422 |
2/4✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
|
1343745 | Eigen::Vector3d e_normalVectorCurrentPosition{ std::cos(lla_position(0)) * std::cos(lla_position(1)), |
| 1423 |
2/4✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
|
1343745 | std::cos(lla_position(0)) * std::sin(lla_position(1)), |
| 1424 |
2/4✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
|
1343745 | std::sin(lla_position(0)) }; |
| 1425 | |||
| 1426 |
1/2✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
|
1343745 | auto yaw = calcYawFromVelocity(n_velocity); |
| 1427 | |||
| 1428 |
3/4✓ Branch 0 taken 1343744 times.
✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1343744 times.
✗ Branch 5 not taken.
|
1343745 | splineYaw[i] = i > 0 ? unwrapAngle(yaw, splineYaw[i - 1], M_PI) : yaw; |
| 1429 | 1343745 | splineRoll[i] = 0.0; | |
| 1430 |
4/8✓ Branch 1 taken 1343745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343745 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1343745 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1343745 times.
✗ Branch 10 not taken.
|
1343745 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.roll.setPoints(splineTime, splineRoll); |
| 1435 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.pitch.setPoints(splineTime, splinePitch); |
| 1436 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.yaw.setPoints(splineTime, splineYaw); |
| 1437 | 1 | } | |
| 1438 | |||
| 1439 | 7 | return true; | |
| 1440 | 7 | } | |
| 1441 | |||
| 1442 | 7 | bool NAV::ImuSimulator::initialize() | |
| 1443 | { | ||
| 1444 | LOG_TRACE("{}: called", nameId()); | ||
| 1445 | |||
| 1446 | 7 | return initializeSplines(); | |
| 1447 | } | ||
| 1448 | |||
| 1449 | 7 | void NAV::ImuSimulator::deinitialize() | |
| 1450 | { | ||
| 1451 | LOG_TRACE("{}: called", nameId()); | ||
| 1452 | 7 | } | |
| 1453 | |||
| 1454 | 14 | bool NAV::ImuSimulator::resetNode() | |
| 1455 | { | ||
| 1456 | LOG_TRACE("{}: called", nameId()); | ||
| 1457 | |||
| 1458 | 14 | _imuInternalUpdateCnt = 0; | |
| 1459 | 14 | _imuUpdateCnt = 0; | |
| 1460 | 14 | _gnssUpdateCnt = 0; | |
| 1461 | |||
| 1462 | 14 | _imuLastUpdateTime = 0.0; | |
| 1463 | 14 | _gnssLastUpdateTime = 0.0; | |
| 1464 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
14 | _lla_imuLastLinearPosition = _startPosition.latLonAlt(); |
| 1465 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
14 | _lla_gnssLastLinearPosition = _startPosition.latLonAlt(); |
| 1466 | |||
| 1467 |
3/6✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
|
14 | _p_lastImuAccelerationMeas = Eigen::Vector3d::Ones() * std::nan(""); |
| 1468 |
3/6✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
|
14 | _p_lastImuAngularRateMeas = Eigen::Vector3d::Ones() * std::nan(""); |
| 1469 | |||
| 1470 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
|
14 | 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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
|
14 | 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 | 14 | return true; | |
| 1499 | } | ||
| 1500 | |||
| 1501 | 117293 | bool NAV::ImuSimulator::checkStopCondition(double time, const Eigen::Vector3d& lla_position) | |
| 1502 | { | ||
| 1503 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 117293 times.
|
117293 | if (_trajectoryType == TrajectoryType::Csv) |
| 1504 | { | ||
| 1505 | ✗ | return time > _csvDuration; | |
| 1506 | } | ||
| 1507 |
2/2✓ Branch 0 taken 96890 times.
✓ Branch 1 taken 20403 times.
|
117293 | if (_simulationStopCondition == StopCondition::Duration |
| 1508 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 96890 times.
|
96890 | || _trajectoryType == TrajectoryType::Fixed) |
| 1509 | { | ||
| 1510 | 20403 | return time > _simulationDuration; | |
| 1511 | } | ||
| 1512 |
1/2✓ Branch 0 taken 96890 times.
✗ Branch 1 not taken.
|
96890 | if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses) |
| 1513 | { | ||
| 1514 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 96890 times.
|
96890 | 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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 96890 times.
|
96890 | if (_trajectoryType == TrajectoryType::Circular) |
| 1522 | { | ||
| 1523 | ✗ | double omega = _trajectoryHorizontalSpeed / _trajectoryRadius; | |
| 1524 | ✗ | double simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega; | |
| 1525 | ✗ | return time > simDuration; | |
| 1526 | } | ||
| 1527 |
1/2✓ Branch 0 taken 96890 times.
✗ Branch 1 not taken.
|
96890 | if (_trajectoryType == TrajectoryType::RoseFigure) |
| 1528 | { | ||
| 1529 | 96890 | return time > _roseSimDuration; | |
| 1530 | } | ||
| 1531 | } | ||
| 1532 | ✗ | return false; | |
| 1533 | } | ||
| 1534 | |||
| 1535 | 68855 | std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollImuObs(size_t /* pinIdx */, bool peek) | |
| 1536 | { | ||
| 1537 | 68855 | double imuUpdateTime = static_cast<double>(_imuUpdateCnt) / _imuFrequency; | |
| 1538 | |||
| 1539 | // -------------------------------------------------- Check if a stop condition is met ----------------------------------------------------- | ||
| 1540 |
4/6✓ Branch 1 taken 68849 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 68845 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✓ Branch 7 taken 68838 times.
|
68855 | if (checkStopCondition(imuUpdateTime, lla_calcPosition(imuUpdateTime))) |
| 1541 | { | ||
| 1542 | 7 | return nullptr; | |
| 1543 | } | ||
| 1544 | // ------------------------------------- Early return in case of peeking to avoid heavy calculations --------------------------------------- | ||
| 1545 |
2/2✓ Branch 0 taken 34418 times.
✓ Branch 1 taken 34420 times.
|
68838 | if (peek) |
| 1546 | { | ||
| 1547 |
1/2✓ Branch 1 taken 34424 times.
✗ Branch 2 not taken.
|
34418 | auto obs = std::make_shared<NodeData>(); |
| 1548 |
2/4✓ Branch 2 taken 34418 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34421 times.
✗ Branch 6 not taken.
|
34424 | obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime); |
| 1549 | 34411 | return obs; | |
| 1550 | 34414 | } | |
| 1551 | |||
| 1552 |
1/2✓ Branch 1 taken 34425 times.
✗ Branch 2 not taken.
|
34420 | auto obs = std::make_shared<ImuObsSimulated>(_imuPos); |
| 1553 |
2/4✓ Branch 2 taken 34419 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34423 times.
✗ Branch 6 not taken.
|
34425 | 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 | 34414 | double dTime = 0.0; | |
| 1558 |
2/4✓ Branch 1 taken 34415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34417 times.
✗ Branch 5 not taken.
|
34414 | Eigen::Vector3d p_deltaVel = Eigen::Vector3d::Zero(); |
| 1559 |
2/4✓ Branch 1 taken 34414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34422 times.
✗ Branch 5 not taken.
|
34417 | Eigen::Vector3d p_deltaTheta = Eigen::Vector3d::Zero(); |
| 1560 | |||
| 1561 | 34422 | double imuInternalUpdateTime = 0.0; | |
| 1562 | do { | ||
| 1563 | 2833089 | 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 |
1/2✓ Branch 1 taken 2831598 times.
✗ Branch 2 not taken.
|
2833089 | 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 |
1/2✓ Branch 1 taken 2832117 times.
✗ Branch 2 not taken.
|
2831598 | Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_position); |
| 1571 |
3/6✓ Branch 1 taken 2829964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2832864 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2838747 times.
✗ Branch 8 not taken.
|
2832117 | Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1)); |
| 1572 | |||
| 1573 |
1/2✓ Branch 1 taken 2832520 times.
✗ Branch 2 not taken.
|
2838747 | 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 |
1/2✓ Branch 1 taken 2839433 times.
✗ Branch 2 not taken.
|
2832520 | auto [roll, pitch, yaw] = calcFlightAngles(imuInternalUpdateTime); |
| 1577 | LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), imuInternalUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw)); | ||
| 1578 | |||
| 1579 |
1/2✓ Branch 1 taken 2835594 times.
✗ Branch 2 not taken.
|
2830517 | Eigen::Quaterniond b_Quat_n = trafo::b_Quat_n(roll, pitch, yaw); |
| 1580 | |||
| 1581 |
1/2✓ Branch 1 taken 2831169 times.
✗ Branch 2 not taken.
|
2835594 | 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 |
2/4✓ Branch 1 taken 2829025 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2823994 times.
✗ Branch 5 not taken.
|
2831169 | auto R_N = calcEarthRadius_N(lla_position(0)); |
| 1584 | LOG_DATA("{}: [{:8.3f}] R_N = {} [m]", nameId(), imuInternalUpdateTime, R_N); | ||
| 1585 |
2/4✓ Branch 1 taken 2831446 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2832714 times.
✗ Branch 5 not taken.
|
2823994 | auto R_E = calcEarthRadius_E(lla_position(0)); |
| 1586 | LOG_DATA("{}: [{:8.3f}] R_E = {} [m]", nameId(), imuInternalUpdateTime, R_E); | ||
| 1587 |
1/2✓ Branch 1 taken 2836593 times.
✗ Branch 2 not taken.
|
2832714 | 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 |
1/2✓ Branch 1 taken 2836458 times.
✗ Branch 2 not taken.
|
2836593 | 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 |
1/2✓ Branch 1 taken 2818321 times.
✗ Branch 2 not taken.
|
2836458 | Eigen::Vector3d n_accel = n_trajectoryAccel; |
| 1598 |
1/2✓ Branch 0 taken 2821480 times.
✗ Branch 1 not taken.
|
2818321 | if (_coriolisAccelerationEnabled) // Apply Coriolis Acceleration |
| 1599 | { | ||
| 1600 |
1/2✓ Branch 1 taken 2832563 times.
✗ Branch 2 not taken.
|
2821480 | 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 |
1/2✓ Branch 1 taken 2826728 times.
✗ Branch 2 not taken.
|
2832563 | n_accel += n_coriolisAcceleration; |
| 1603 | } | ||
| 1604 | |||
| 1605 | // Mass attraction of the Earth (gravitation) | ||
| 1606 |
1/2✓ Branch 1 taken 2826921 times.
✗ Branch 2 not taken.
|
2823569 | 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 |
1/2✓ Branch 1 taken 2825269 times.
✗ Branch 2 not taken.
|
2826921 | n_accel -= n_gravitation; // Apply the local gravity vector |
| 1609 | |||
| 1610 |
1/2✓ Branch 0 taken 2825427 times.
✗ Branch 1 not taken.
|
2825269 | if (_centrifgalAccelerationEnabled) // Centrifugal acceleration caused by the Earth's rotation |
| 1611 | { | ||
| 1612 |
1/2✓ Branch 1 taken 2834150 times.
✗ Branch 2 not taken.
|
2825427 | Eigen::Vector3d e_centrifugalAcceleration = e_calcCentrifugalAcceleration(e_position); |
| 1613 | LOG_DATA("{}: [{:8.3f}] e_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, e_centrifugalAcceleration.transpose()); | ||
| 1614 |
1/2✓ Branch 1 taken 2831008 times.
✗ Branch 2 not taken.
|
2834150 | 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 |
1/2✓ Branch 1 taken 2829474 times.
✗ Branch 2 not taken.
|
2831008 | n_accel += n_centrifugalAcceleration; |
| 1617 | } | ||
| 1618 | |||
| 1619 | // Acceleration measured by the accelerometer in platform coordinates | ||
| 1620 |
3/6✓ Branch 1 taken 2833054 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2836891 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2831060 times.
✗ Branch 8 not taken.
|
2829316 | 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 |
3/6✓ Branch 1 taken 2834063 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2832765 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2835968 times.
✗ Branch 8 not taken.
|
2831060 | 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 |
1/2✓ Branch 1 taken 2825632 times.
✗ Branch 2 not taken.
|
2835968 | Eigen::Vector3d n_omega_ib = n_omega_nb; |
| 1629 |
1/2✓ Branch 0 taken 2828320 times.
✗ Branch 1 not taken.
|
2825632 | if (_angularRateEarthRotationEnabled) |
| 1630 | { | ||
| 1631 |
1/2✓ Branch 1 taken 2829117 times.
✗ Branch 2 not taken.
|
2828320 | n_omega_ib += n_omega_ie; |
| 1632 | } | ||
| 1633 |
1/2✓ Branch 0 taken 2829705 times.
✗ Branch 1 not taken.
|
2826429 | if (_angularRateTransportRateEnabled) |
| 1634 | { | ||
| 1635 |
1/2✓ Branch 1 taken 2832595 times.
✗ Branch 2 not taken.
|
2829705 | 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 |
3/6✓ Branch 1 taken 2832947 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2837140 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2830394 times.
✗ Branch 8 not taken.
|
2829319 | 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 |
2/4✓ Branch 1 taken 2826892 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2823714 times.
✗ Branch 6 not taken.
|
2830394 | obs->p_acceleration = p_accel.cast<double>(); |
| 1647 |
2/4✓ Branch 1 taken 2821963 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2831717 times.
✗ Branch 6 not taken.
|
2823714 | obs->p_angularRate = p_omega_ip.cast<double>(); |
| 1648 | // obs->p_magneticField.emplace(0, 0, 0); | ||
| 1649 | |||
| 1650 |
1/2✓ Branch 1 taken 2833622 times.
✗ Branch 2 not taken.
|
2831717 | Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate(); |
| 1651 | |||
| 1652 |
2/4✓ Branch 1 taken 2829981 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2831150 times.
✗ Branch 6 not taken.
|
2833622 | obs->n_accelDynamics = n_trajectoryAccel.cast<double>(); |
| 1653 |
2/4✓ Branch 1 taken 2829270 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2833193 times.
✗ Branch 6 not taken.
|
2831150 | obs->n_angularRateDynamics = n_omega_nb.cast<double>(); |
| 1654 | |||
| 1655 |
3/6✓ Branch 1 taken 2830661 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2828890 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2830679 times.
✗ Branch 9 not taken.
|
2833193 | obs->e_accelDynamics = (e_Quat_n * n_trajectoryAccel).cast<double>(); |
| 1656 |
3/6✓ Branch 1 taken 2831940 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2830016 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2831765 times.
✗ Branch 9 not taken.
|
2830679 | obs->e_angularRateDynamics = (e_Quat_n * n_omega_nb).cast<double>(); |
| 1657 | |||
| 1658 | 2831765 | double dt = 1.0 / _imuInternalFrequency; | |
| 1659 |
2/2✓ Branch 0 taken 2831124 times.
✓ Branch 1 taken 641 times.
|
2831765 | if (_imuInternalUpdateCnt != 0) |
| 1660 | { | ||
| 1661 | 2831124 | dTime += dt; | |
| 1662 | |||
| 1663 |
4/8✓ Branch 1 taken 2834080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2825409 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2827862 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2829516 times.
✗ Branch 11 not taken.
|
2831124 | p_deltaVel += (p_accel + _p_lastImuAccelerationMeas) / 2 * dt; |
| 1664 |
4/8✓ Branch 1 taken 2832380 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2827722 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2829840 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2833074 times.
✗ Branch 11 not taken.
|
2829516 | p_deltaTheta += (p_omega_ip + _p_lastImuAngularRateMeas) / 2 * dt; |
| 1665 | } | ||
| 1666 |
1/2✓ Branch 1 taken 2828685 times.
✗ Branch 2 not taken.
|
2833715 | _p_lastImuAccelerationMeas = p_accel; |
| 1667 |
1/2✓ Branch 1 taken 2833087 times.
✗ Branch 2 not taken.
|
2828685 | _p_lastImuAngularRateMeas = p_omega_ip; |
| 1668 | |||
| 1669 | 2833087 | _imuInternalUpdateCnt++; | |
| 1670 |
2/2✓ Branch 0 taken 2798667 times.
✓ Branch 1 taken 34420 times.
|
2833087 | } while (imuInternalUpdateTime + 1e-6 < imuUpdateTime); |
| 1671 | |||
| 1672 | 34420 | obs->dtime = dTime; | |
| 1673 |
2/4✓ Branch 1 taken 34416 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 34418 times.
✗ Branch 6 not taken.
|
34420 | obs->dvel = p_deltaVel.cast<double>(); |
| 1674 |
2/4✓ Branch 1 taken 34420 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 34424 times.
✗ Branch 6 not taken.
|
34418 | obs->dtheta = p_deltaTheta.cast<double>(); |
| 1675 | LOG_DATA("{}: dtime = {}, dvel = {}, dtheta = {}", nameId(), obs->dtime, obs->dvel.transpose(), obs->dtheta.transpose()); | ||
| 1676 | |||
| 1677 | 34424 | _imuUpdateCnt++; | |
| 1678 |
1/2✓ Branch 2 taken 34422 times.
✗ Branch 3 not taken.
|
34424 | invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs); |
| 1679 | |||
| 1680 | 34423 | return obs; | |
| 1681 | 34422 | } | |
| 1682 | |||
| 1683 | 48445 | std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollPosVelAtt(size_t /* pinIdx */, bool peek) | |
| 1684 | { | ||
| 1685 | 48445 | auto gnssUpdateTime = static_cast<double>(_gnssUpdateCnt) / _gnssFrequency; | |
| 1686 | |||
| 1687 |
1/2✓ Branch 1 taken 48445 times.
✗ Branch 2 not taken.
|
48445 | Eigen::Vector3d lla_position = lla_calcPosition(gnssUpdateTime); |
| 1688 | |||
| 1689 | // -------------------------------------------------- Check if a stop condition is met ----------------------------------------------------- | ||
| 1690 |
3/4✓ Branch 1 taken 48445 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 48444 times.
|
48445 | if (checkStopCondition(gnssUpdateTime, lla_position)) |
| 1691 | { | ||
| 1692 | 1 | return nullptr; | |
| 1693 | } | ||
| 1694 | |||
| 1695 | // ------------------------------------- Early return in case of peeking to avoid heavy calculations --------------------------------------- | ||
| 1696 |
2/2✓ Branch 0 taken 24222 times.
✓ Branch 1 taken 24222 times.
|
48444 | if (peek) |
| 1697 | { | ||
| 1698 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | auto obs = std::make_shared<NodeData>(); |
| 1699 |
2/4✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
|
24222 | obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime); |
| 1700 | 24222 | return obs; | |
| 1701 | 24222 | } | |
| 1702 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | auto obs = std::make_shared<PosVelAtt>(); |
| 1703 |
2/4✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
|
24222 | 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 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1)); |
| 1709 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | 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 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | 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 |
5/10✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24222 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24222 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24222 times.
✗ Branch 15 not taken.
|
24222 | obs->setPosVelAtt_n(lla_position.cast<double>(), n_vel.cast<double>(), trafo::n_Quat_b(roll, pitch, yaw).cast<double>()); |
| 1717 | |||
| 1718 | 24222 | _gnssUpdateCnt++; | |
| 1719 |
1/2✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
|
24222 | invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, obs); |
| 1720 | |||
| 1721 | 24222 | return obs; | |
| 1722 | 24222 | } | |
| 1723 | |||
| 1724 | 2857629 | std::array<double, 3> NAV::ImuSimulator::calcFlightAngles(double time) const | |
| 1725 | { | ||
| 1726 | return { | ||
| 1727 | 2857629 | static_cast<double>(_splines.roll(time)), | |
| 1728 | 5720986 | static_cast<double>(_splines.pitch(time)), | |
| 1729 | 5726246 | static_cast<double>(_splines.yaw(time)) | |
| 1730 | 2858623 | }; | |
| 1731 | } | ||
| 1732 | |||
| 1733 | 2954446 | Eigen::Vector3d NAV::ImuSimulator::lla_calcPosition(double time) const | |
| 1734 | { | ||
| 1735 | ✗ | Eigen::Vector3d e_pos(static_cast<double>(_splines.x(time)), | |
| 1736 |
1/2✓ Branch 1 taken 2956389 times.
✗ Branch 2 not taken.
|
2955913 | static_cast<double>(_splines.y(time)), |
| 1737 |
3/6✓ Branch 1 taken 2947903 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2955913 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2953067 times.
✗ Branch 8 not taken.
|
5910835 | static_cast<double>(_splines.z(time))); |
| 1738 |
1/2✓ Branch 1 taken 2949420 times.
✗ Branch 2 not taken.
|
5902487 | return trafo::ecef2lla_WGS84(e_pos); |
| 1739 | } | ||
| 1740 | |||
| 1741 | 2862354 | Eigen::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 |
1/2✓ Branch 1 taken 2863490 times.
✗ Branch 2 not taken.
|
2862487 | static_cast<double>(_splines.y.derivative(1, time)), |
| 1745 |
3/6✓ Branch 1 taken 2856953 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2862487 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2857315 times.
✗ Branch 8 not taken.
|
5725844 | static_cast<double>(_splines.z.derivative(1, time))); |
| 1746 |
1/2✓ Branch 1 taken 2853907 times.
✗ Branch 2 not taken.
|
5711222 | return n_Quat_e * e_vel; |
| 1747 | } | ||
| 1748 | |||
| 1749 | 2837826 | Eigen::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 |
1/2✓ Branch 1 taken 2839580 times.
✗ Branch 2 not taken.
|
2839020 | static_cast<double>(_splines.y.derivative(2, time)), |
| 1756 |
3/6✓ Branch 1 taken 2833511 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2839020 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2836386 times.
✗ Branch 8 not taken.
|
5677406 | static_cast<double>(_splines.z.derivative(2, time))); |
| 1757 |
1/2✓ Branch 1 taken 2833819 times.
✗ Branch 2 not taken.
|
2836386 | Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate(); |
| 1758 |
1/2✓ Branch 1 taken 2835268 times.
✗ Branch 2 not taken.
|
2833819 | Eigen::Vector3d e_vel = e_Quat_n * n_velocity; |
| 1759 | |||
| 1760 | // Math: \dot{C}_n^e = C_n^e \cdot \Omega_{en}^n | ||
| 1761 |
1/2✓ Branch 1 taken 2836017 times.
✗ Branch 2 not taken.
|
2838725 | Eigen::Matrix3d n_DCM_dot_e = e_Quat_n.toRotationMatrix() |
| 1762 |
1/2✓ Branch 1 taken 2838725 times.
✗ Branch 2 not taken.
|
2837628 | * math::skewSymmetricMatrix(n_calcTransportRate(lla_position, n_velocity, |
| 1763 |
2/4✓ Branch 1 taken 2836495 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2837628 times.
✗ Branch 5 not taken.
|
2832019 | calcEarthRadius_N(lla_position(0)), |
| 1764 |
5/10✓ Branch 1 taken 2827290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2830529 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2832019 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2821963 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2835850 times.
✗ Branch 14 not taken.
|
5671285 | calcEarthRadius_E(lla_position(0)))); |
| 1765 | |||
| 1766 | // Math: \dot{C}_e^n = (\dot{C}_n^e)^T | ||
| 1767 |
2/4✓ Branch 1 taken 2813951 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2824397 times.
✗ Branch 5 not taken.
|
2835850 | 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 |
4/8✓ Branch 1 taken 2831262 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2820076 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2826362 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2826489 times.
✗ Branch 11 not taken.
|
2824397 | return e_DCM_dot_n * e_vel + n_Quat_e * e_accel; |
| 1771 | } | ||
| 1772 | |||
| 1773 | 2830518 | Eigen::Vector3d NAV::ImuSimulator::n_calcOmega_nb(double time, const Eigen::Vector3d& rollPitchYaw, const Eigen::Quaterniond& n_Quat_b) const | |
| 1774 | { | ||
| 1775 |
1/2✓ Branch 1 taken 2827272 times.
✗ Branch 2 not taken.
|
2830518 | const auto& R = rollPitchYaw(0); |
| 1776 |
1/2✓ Branch 1 taken 2831933 times.
✗ Branch 2 not taken.
|
2827272 | const auto& P = rollPitchYaw(1); |
| 1777 | |||
| 1778 | // ######################################################################################################################################### | ||
| 1779 | |||
| 1780 |
1/2✓ Branch 1 taken 2832516 times.
✗ Branch 2 not taken.
|
2831933 | auto R_dot = static_cast<double>(_splines.roll.derivative(1, time)); |
| 1781 |
1/2✓ Branch 1 taken 2837842 times.
✗ Branch 2 not taken.
|
2832516 | auto Y_dot = static_cast<double>(_splines.yaw.derivative(1, time)); |
| 1782 |
1/2✓ Branch 1 taken 2838921 times.
✗ Branch 2 not taken.
|
2837842 | auto P_dot = static_cast<double>(_splines.pitch.derivative(1, time)); |
| 1783 | |||
| 1784 | 5659897 | 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 |
2/4✓ Branch 1 taken 5650876 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5665578 times.
✗ Branch 5 not taken.
|
5659897 | return Eigen::AngleAxisd{ -R, Eigen::Vector3d::UnitX() }; |
| 1791 | }; | ||
| 1792 | 2835143 | 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 |
2/4✓ Branch 1 taken 2822700 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2827772 times.
✗ Branch 5 not taken.
|
2835143 | 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 |
1/2✓ Branch 1 taken 2827059 times.
✗ Branch 2 not taken.
|
2829425 | Eigen::Vector3d b_omega_nb = Eigen::Vector3d{ R_dot, 0, 0 } |
| 1805 |
4/8✓ Branch 1 taken 2833552 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2831591 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2829425 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2826268 times.
✗ Branch 11 not taken.
|
5658468 | + C_3(R) * Eigen::Vector3d{ 0, P_dot, 0 } |
| 1806 |
7/14✓ Branch 1 taken 2836915 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2830211 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2834827 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2837484 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2831409 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2826626 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2825758 times.
✗ Branch 20 not taken.
|
5665189 | + C_3(R) * C_2(P) * Eigen::Vector3d{ 0, 0, Y_dot }; |
| 1807 | |||
| 1808 |
1/2✓ Branch 1 taken 2831153 times.
✗ Branch 2 not taken.
|
5656911 | return n_Quat_b * b_omega_nb; |
| 1809 | } | ||
| 1810 | |||
| 1811 | ✗ | const 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 | |||
| 1831 | ✗ | const 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 | } | ||
| 1844 |