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