| 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 <ctime> | ||
| 12 | |||
| 13 | #include "util/Logger.hpp" | ||
| 14 | #include "util/StringUtil.hpp" | ||
| 15 | #include "Navigation/Ellipsoid/Ellipsoid.hpp" | ||
| 16 | #include "Navigation/INS/Functions.hpp" | ||
| 17 | #include "Navigation/INS/LocalNavFrame/Mechanization.hpp" | ||
| 18 | #include "Navigation/Gravity/Gravity.hpp" | ||
| 19 | #include "Navigation/Math/NumericalIntegration.hpp" | ||
| 20 | #include "Navigation/Math/Math.hpp" | ||
| 21 | #include "Navigation/Transformations/Units.hpp" | ||
| 22 | #include "util/Time/TimeBase.hpp" | ||
| 23 | |||
| 24 | #include "internal/NodeManager.hpp" | ||
| 25 | #include <Eigen/src/Geometry/AngleAxis.h> | ||
| 26 | namespace nm = NAV::NodeManager; | ||
| 27 | #include "internal/FlowManager.hpp" | ||
| 28 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 29 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 30 | #include "internal/gui/NodeEditorApplication.hpp" | ||
| 31 | |||
| 32 | #include "NodeData/IMU/ImuObsSimulated.hpp" | ||
| 33 | #include "NodeData/State/PosVelAtt.hpp" | ||
| 34 | |||
| 35 | 120 | NAV::ImuSimulator::ImuSimulator() | |
| 36 |
17/34✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
✓ Branch 10 taken 120 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 120 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 120 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 120 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 120 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 120 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 120 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 120 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 120 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 120 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 120 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 120 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 120 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 120 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 120 times.
✗ Branch 54 not taken.
|
120 | : Imu(typeStatic()) |
| 37 | { | ||
| 38 | LOG_TRACE("{}: called", name); | ||
| 39 | |||
| 40 | 120 | _hasConfig = true; | |
| 41 | 120 | _guiConfigDefaultWindowSize = { 677, 508 }; | |
| 42 | |||
| 43 |
4/8✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 120 times.
✓ Branch 9 taken 120 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
360 | nm::CreateOutputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObsSimulated::type() }, &ImuSimulator::pollImuObs); |
| 44 |
4/8✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 120 times.
✓ Branch 9 taken 120 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
360 | nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &ImuSimulator::pollPosVelAtt); |
| 45 | 360 | } | |
| 46 | |||
| 47 | 256 | NAV::ImuSimulator::~ImuSimulator() | |
| 48 | { | ||
| 49 | LOG_TRACE("{}: called", nameId()); | ||
| 50 | 256 | } | |
| 51 | |||
| 52 | 232 | std::string NAV::ImuSimulator::typeStatic() | |
| 53 | { | ||
| 54 |
1/2✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
|
464 | return "ImuSimulator"; |
| 55 | } | ||
| 56 | |||
| 57 | ✗ | std::string NAV::ImuSimulator::type() const | |
| 58 | { | ||
| 59 | ✗ | return typeStatic(); | |
| 60 | } | ||
| 61 | |||
| 62 | 112 | std::string NAV::ImuSimulator::category() | |
| 63 | { | ||
| 64 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Simulator"; |
| 65 | } | ||
| 66 | |||
| 67 | ✗ | void NAV::ImuSimulator::guiConfig() | |
| 68 | { | ||
| 69 | ✗ | float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio(); | |
| 70 | |||
| 71 | ✗ | if (_trajectoryType != TrajectoryType::Csv && ImGui::TreeNode("Start Time")) | |
| 72 | { | ||
| 73 | ✗ | auto startTimeSource = static_cast<int>(_startTimeSource); | |
| 74 | ✗ | if (ImGui::RadioButton(fmt::format("Current Computer Time##{}", size_t(id)).c_str(), &startTimeSource, static_cast<int>(StartTimeSource::CurrentComputerTime))) | |
| 75 | { | ||
| 76 | ✗ | _startTimeSource = static_cast<decltype(_startTimeSource)>(startTimeSource); | |
| 77 | ✗ | LOG_DEBUG("{}: startTimeSource changed to {}", nameId(), fmt::underlying(_startTimeSource)); | |
| 78 | ✗ | flow::ApplyChanges(); | |
| 79 | } | ||
| 80 | ✗ | if (_startTimeSource == StartTimeSource::CurrentComputerTime) | |
| 81 | { | ||
| 82 | ✗ | ImGui::Indent(); | |
| 83 | |||
| 84 | ✗ | std::time_t t = std::time(nullptr); | |
| 85 | ✗ | std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe) | |
| 86 | |||
| 87 | ✗ | 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); | |
| 88 | |||
| 89 | ✗ | ImGui::Unindent(); | |
| 90 | } | ||
| 91 | |||
| 92 | ✗ | if (ImGui::RadioButton(fmt::format("Custom Time##{}", size_t(id)).c_str(), &startTimeSource, static_cast<int>(StartTimeSource::CustomTime))) | |
| 93 | { | ||
| 94 | ✗ | _startTimeSource = static_cast<decltype(_startTimeSource)>(startTimeSource); | |
| 95 | ✗ | LOG_DEBUG("{}: startTimeSource changed to {}", nameId(), fmt::underlying(_startTimeSource)); | |
| 96 | ✗ | flow::ApplyChanges(); | |
| 97 | } | ||
| 98 | ✗ | if (_startTimeSource == StartTimeSource::CustomTime) | |
| 99 | { | ||
| 100 | ✗ | ImGui::Indent(); | |
| 101 | ✗ | if (gui::widgets::TimeEdit(fmt::format("{}", size_t(id)).c_str(), _startTime, _startTimeEditFormat)) | |
| 102 | { | ||
| 103 | ✗ | LOG_DEBUG("{}: startTime changed to {}", nameId(), _startTime); | |
| 104 | ✗ | flow::ApplyChanges(); | |
| 105 | } | ||
| 106 | ✗ | ImGui::Unindent(); | |
| 107 | } | ||
| 108 | ✗ | ImGui::TreePop(); | |
| 109 | } | ||
| 110 | |||
| 111 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 112 | ✗ | if (ImGui::TreeNode("Output data-rate")) | |
| 113 | { | ||
| 114 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 115 | ✗ | if (ImGui::InputDoubleL(fmt::format("IMU internal rate##{}", size_t(id)).c_str(), &_imuInternalFrequency, 1e-3, 1e4, 0.0, 0.0, "%.3f Hz")) | |
| 116 | { | ||
| 117 | ✗ | LOG_DEBUG("{}: imuInternalFrequency changed to {}", nameId(), _imuInternalFrequency); | |
| 118 | ✗ | flow::ApplyChanges(); | |
| 119 | } | ||
| 120 | ✗ | ImGui::SameLine(); | |
| 121 | ✗ | gui::widgets::HelpMarker("Used to calculate the dTime, dTheta, dVel fields"); | |
| 122 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 123 | ✗ | if (ImGui::InputDoubleL(fmt::format("IMU output rate##{}", size_t(id)).c_str(), &_imuFrequency, 1e-3, _imuInternalFrequency, 0.0, 0.0, "%.3f Hz")) | |
| 124 | { | ||
| 125 | ✗ | LOG_DEBUG("{}: imuFrequency changed to {}", nameId(), _imuFrequency); | |
| 126 | ✗ | flow::ApplyChanges(); | |
| 127 | } | ||
| 128 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 129 | ✗ | if (ImGui::InputDouble(fmt::format("Trajectory output rate##{}", size_t(id)).c_str(), &_gnssFrequency, 0.0, 0.0, "%.3f Hz")) | |
| 130 | { | ||
| 131 | ✗ | LOG_DEBUG("{}: gnssFrequency changed to {}", nameId(), _gnssFrequency); | |
| 132 | ✗ | flow::ApplyChanges(); | |
| 133 | } | ||
| 134 | |||
| 135 | ✗ | ImGui::TreePop(); | |
| 136 | } | ||
| 137 | |||
| 138 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 139 | ✗ | if (ImGui::TreeNode("Scenario")) | |
| 140 | { | ||
| 141 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 142 | ✗ | if (ImGui::BeginCombo(fmt::format("Trajectory##{}", size_t(id)).c_str(), to_string(_trajectoryType))) | |
| 143 | { | ||
| 144 | ✗ | for (size_t i = 0; i < static_cast<size_t>(TrajectoryType::COUNT); i++) | |
| 145 | { | ||
| 146 | ✗ | const bool is_selected = (static_cast<size_t>(_trajectoryType) == i); | |
| 147 | ✗ | if (ImGui::Selectable(to_string(static_cast<TrajectoryType>(i)), is_selected)) | |
| 148 | { | ||
| 149 | ✗ | _trajectoryType = static_cast<TrajectoryType>(i); | |
| 150 | ✗ | LOG_DEBUG("{}: trajectoryType changed to {}", nameId(), fmt::underlying(_trajectoryType)); | |
| 151 | |||
| 152 | ✗ | if (_trajectoryType == TrajectoryType::Csv && inputPins.empty()) | |
| 153 | { | ||
| 154 | ✗ | nm::CreateInputPin(this, CsvData::type().c_str(), Pin::Type::Object, { CsvData::type() }); | |
| 155 | } | ||
| 156 | ✗ | else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty()) | |
| 157 | { | ||
| 158 | ✗ | nm::DeleteInputPin(inputPins.front()); | |
| 159 | } | ||
| 160 | |||
| 161 | ✗ | flow::ApplyChanges(); | |
| 162 | ✗ | doDeinitialize(); | |
| 163 | } | ||
| 164 | |||
| 165 | ✗ | if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus) | |
| 166 | { | ||
| 167 | ✗ | ImGui::SetItemDefaultFocus(); | |
| 168 | } | ||
| 169 | } | ||
| 170 | ✗ | ImGui::EndCombo(); | |
| 171 | } | ||
| 172 | ✗ | if (_trajectoryType == TrajectoryType::Csv) | |
| 173 | { | ||
| 174 | ✗ | auto TextColoredIfExists = [this](int index, const char* text, const char* type) { | |
| 175 | ✗ | ImGui::TableSetColumnIndex(index); | |
| 176 | ✗ | auto displayTable = [&]() { | |
| 177 | ✗ | if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV); | |
| 178 | ✗ | csvData && std::ranges::find(csvData->v->description, text) != csvData->v->description.end()) | |
| 179 | { | ||
| 180 | ✗ | ImGui::TextUnformatted(text); | |
| 181 | ✗ | ImGui::TableNextColumn(); | |
| 182 | ✗ | ImGui::TextUnformatted(type); | |
| 183 | } | ||
| 184 | else | ||
| 185 | { | ||
| 186 | ✗ | ImGui::TextDisabled("%s", text); | |
| 187 | ✗ | ImGui::TableNextColumn(); | |
| 188 | ✗ | ImGui::TextDisabled("%s", type); | |
| 189 | ✗ | } | |
| 190 | ✗ | }; | |
| 191 | ✗ | displayTable(); | |
| 192 | ✗ | }; | |
| 193 | |||
| 194 | ✗ | if (ImGui::TreeNode(fmt::format("Format description##{}", size_t(id)).c_str())) | |
| 195 | { | ||
| 196 | ✗ | ImGui::TextUnformatted("Time information:"); | |
| 197 | ✗ | ImGui::SameLine(); | |
| 198 | ✗ | gui::widgets::HelpMarker("You can either provide the time in GPS time or in UTC time."); | |
| 199 | ✗ | if (ImGui::BeginTable(fmt::format("##Time ({})", size_t(id)).c_str(), 5, | |
| 200 | ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX)) | ||
| 201 | { | ||
| 202 | ✗ | ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed); | |
| 203 | ✗ | ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed); | |
| 204 | ✗ | ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed); | |
| 205 | ✗ | ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed); | |
| 206 | ✗ | ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed); | |
| 207 | ✗ | ImGui::TableHeadersRow(); | |
| 208 | |||
| 209 | ✗ | ImGui::TableNextRow(); | |
| 210 | ✗ | TextColoredIfExists(0, "GpsCycle", "uint"); | |
| 211 | ✗ | TextColoredIfExists(3, "YearUTC", "uint"); | |
| 212 | ✗ | ImGui::TableNextRow(); | |
| 213 | ✗ | TextColoredIfExists(0, "GpsWeek", "uint"); | |
| 214 | ✗ | TextColoredIfExists(3, "MonthUTC", "uint"); | |
| 215 | ✗ | ImGui::TableNextRow(); | |
| 216 | ✗ | TextColoredIfExists(0, "GpsToW [s]", "uint"); | |
| 217 | ✗ | TextColoredIfExists(3, "DayUTC", "uint"); | |
| 218 | ✗ | ImGui::TableNextRow(); | |
| 219 | ✗ | TextColoredIfExists(3, "HourUTC", "uint"); | |
| 220 | ✗ | ImGui::TableNextRow(); | |
| 221 | ✗ | TextColoredIfExists(3, "MinUTC", "uint"); | |
| 222 | ✗ | ImGui::TableNextRow(); | |
| 223 | ✗ | TextColoredIfExists(3, "SecUTC", "double"); | |
| 224 | |||
| 225 | ✗ | ImGui::EndTable(); | |
| 226 | } | ||
| 227 | |||
| 228 | ✗ | ImGui::TextUnformatted("Position information:"); | |
| 229 | ✗ | ImGui::SameLine(); | |
| 230 | ✗ | gui::widgets::HelpMarker("You can either provide the position in ECEF coordinates\nor in latitude, longitude and altitude."); | |
| 231 | ✗ | if (ImGui::BeginTable(fmt::format("##Position ({})", size_t(id)).c_str(), 5, | |
| 232 | ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX)) | ||
| 233 | { | ||
| 234 | ✗ | ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed); | |
| 235 | ✗ | ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed); | |
| 236 | ✗ | ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed); | |
| 237 | ✗ | ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed); | |
| 238 | ✗ | ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed); | |
| 239 | ✗ | ImGui::TableHeadersRow(); | |
| 240 | |||
| 241 | ✗ | ImGui::TableNextRow(); | |
| 242 | ✗ | TextColoredIfExists(0, "Pos ECEF X [m]", "double"); | |
| 243 | ✗ | TextColoredIfExists(3, "Latitude [deg]", "double"); | |
| 244 | ✗ | ImGui::TableNextRow(); | |
| 245 | ✗ | TextColoredIfExists(0, "Pos ECEF Y [m]", "double"); | |
| 246 | ✗ | TextColoredIfExists(3, "Longitude [deg]", "double"); | |
| 247 | ✗ | ImGui::TableNextRow(); | |
| 248 | ✗ | TextColoredIfExists(0, "Pos ECEF Z [m]", "double"); | |
| 249 | ✗ | TextColoredIfExists(3, "Altitude [m]", "double"); | |
| 250 | |||
| 251 | ✗ | ImGui::EndTable(); | |
| 252 | } | ||
| 253 | |||
| 254 | ✗ | ImGui::TextUnformatted("Attitude information:"); | |
| 255 | ✗ | ImGui::SameLine(); | |
| 256 | ✗ | gui::widgets::HelpMarker("This is optional. If not provided the simulator will calculate\nit using a rotation minimizing frame.\n\n" | |
| 257 | "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 | 8 | void NAV::ImuSimulator::restore(json const& j) | |
| 699 | { | ||
| 700 | LOG_TRACE("{}: called", nameId()); | ||
| 701 | |||
| 702 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("startTimeSource")) |
| 703 | { | ||
| 704 | 8 | j.at("startTimeSource").get_to(_startTimeSource); | |
| 705 | } | ||
| 706 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("startTimeEditFormat")) |
| 707 | { | ||
| 708 | 8 | j.at("startTimeEditFormat").get_to(_startTimeEditFormat); | |
| 709 | } | ||
| 710 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("startTime")) |
| 711 | { | ||
| 712 | 8 | j.at("startTime").get_to(_startTime); | |
| 713 | } | ||
| 714 | // ########################################################################################################### | ||
| 715 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("imuInternalFrequency")) |
| 716 | { | ||
| 717 | 8 | j.at("imuInternalFrequency").get_to(_imuInternalFrequency); | |
| 718 | } | ||
| 719 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("imuFrequency")) |
| 720 | { | ||
| 721 | 8 | j.at("imuFrequency").get_to(_imuFrequency); | |
| 722 | } | ||
| 723 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("gnssFrequency")) |
| 724 | { | ||
| 725 | 8 | j.at("gnssFrequency").get_to(_gnssFrequency); | |
| 726 | } | ||
| 727 | // ########################################################################################################### | ||
| 728 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("trajectoryType")) |
| 729 | { | ||
| 730 | 8 | j.at("trajectoryType").get_to(_trajectoryType); | |
| 731 | |||
| 732 |
2/6✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 8 times.
|
8 | 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 8 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8 times.
|
8 | else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty()) |
| 737 | { | ||
| 738 | ✗ | nm::DeleteInputPin(inputPins.front()); | |
| 739 | } | ||
| 740 | } | ||
| 741 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("startPosition")) |
| 742 | { | ||
| 743 | 8 | j.at("startPosition").get_to(_startPosition); | |
| 744 | } | ||
| 745 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("fixedTrajectoryStartOrientation")) |
| 746 | { | ||
| 747 | 8 | j.at("fixedTrajectoryStartOrientation").get_to(_fixedTrajectoryStartOrientation); | |
| 748 | } | ||
| 749 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("n_linearTrajectoryStartVelocity")) |
| 750 | { | ||
| 751 | 8 | j.at("n_linearTrajectoryStartVelocity").get_to(_n_linearTrajectoryStartVelocity); | |
| 752 | } | ||
| 753 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("circularHarmonicFrequency")) |
| 754 | { | ||
| 755 | 8 | j.at("circularHarmonicFrequency").get_to(_circularHarmonicFrequency); | |
| 756 | } | ||
| 757 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("circularHarmonicAmplitudeFactor")) |
| 758 | { | ||
| 759 | 8 | j.at("circularHarmonicAmplitudeFactor").get_to(_circularHarmonicAmplitudeFactor); | |
| 760 | } | ||
| 761 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("trajectoryHorizontalSpeed")) |
| 762 | { | ||
| 763 | 8 | j.at("trajectoryHorizontalSpeed").get_to(_trajectoryHorizontalSpeed); | |
| 764 | } | ||
| 765 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("trajectoryVerticalSpeed")) |
| 766 | { | ||
| 767 | 8 | j.at("trajectoryVerticalSpeed").get_to(_trajectoryVerticalSpeed); | |
| 768 | } | ||
| 769 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("trajectoryRadius")) |
| 770 | { | ||
| 771 | 8 | j.at("trajectoryRadius").get_to(_trajectoryRadius); | |
| 772 | } | ||
| 773 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("trajectoryRotationAngle")) |
| 774 | { | ||
| 775 | 8 | j.at("trajectoryRotationAngle").get_to(_trajectoryRotationAngle); | |
| 776 | } | ||
| 777 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("trajectoryDirection")) |
| 778 | { | ||
| 779 | 8 | j.at("trajectoryDirection").get_to(_trajectoryDirection); | |
| 780 | } | ||
| 781 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("rosePetNum")) |
| 782 | { | ||
| 783 | 8 | j.at("rosePetNum").get_to(_rosePetNum); | |
| 784 | } | ||
| 785 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("rosePetDenom")) |
| 786 | { | ||
| 787 | 8 | j.at("rosePetDenom").get_to(_rosePetDenom); | |
| 788 | } | ||
| 789 | // ########################################################################################################### | ||
| 790 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("simulationStopCondition")) |
| 791 | { | ||
| 792 | 8 | j.at("simulationStopCondition").get_to(_simulationStopCondition); | |
| 793 | } | ||
| 794 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("simulationDuration")) |
| 795 | { | ||
| 796 | 8 | j.at("simulationDuration").get_to(_simulationDuration); | |
| 797 | } | ||
| 798 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("linearTrajectoryDistanceForStop")) |
| 799 | { | ||
| 800 | 8 | j.at("linearTrajectoryDistanceForStop").get_to(_linearTrajectoryDistanceForStop); | |
| 801 | } | ||
| 802 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("circularTrajectoryCircleCountForStop")) |
| 803 | { | ||
| 804 | 8 | j.at("circularTrajectoryCircleCountForStop").get_to(_circularTrajectoryCircleCountForStop); | |
| 805 | } | ||
| 806 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("roseTrajectoryCountForStop")) |
| 807 | { | ||
| 808 | 8 | j.at("roseTrajectoryCountForStop").get_to(_roseTrajectoryCountForStop); | |
| 809 | } | ||
| 810 | // ########################################################################################################### | ||
| 811 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("splineSampleInterval")) |
| 812 | { | ||
| 813 | 8 | j.at("splineSampleInterval").get_to(_splines.sampleInterval); | |
| 814 | } | ||
| 815 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("roseStepLengthMax")) |
| 816 | { | ||
| 817 | 8 | j.at("roseStepLengthMax").get_to(_roseStepLengthMax); | |
| 818 | } | ||
| 819 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("gravitationModel")) |
| 820 | { | ||
| 821 | 8 | j.at("gravitationModel").get_to(_gravitationModel); | |
| 822 | } | ||
| 823 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("coriolisAccelerationEnabled")) |
| 824 | { | ||
| 825 | 8 | j.at("coriolisAccelerationEnabled").get_to(_coriolisAccelerationEnabled); | |
| 826 | } | ||
| 827 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("centrifgalAccelerationEnabled")) |
| 828 | { | ||
| 829 | 8 | j.at("centrifgalAccelerationEnabled").get_to(_centrifgalAccelerationEnabled); | |
| 830 | } | ||
| 831 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("angularRateEarthRotationEnabled")) |
| 832 | { | ||
| 833 | 8 | j.at("angularRateEarthRotationEnabled").get_to(_angularRateEarthRotationEnabled); | |
| 834 | } | ||
| 835 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("angularRateTransportRateEnabled")) |
| 836 | { | ||
| 837 | 8 | j.at("angularRateTransportRateEnabled").get_to(_angularRateTransportRateEnabled); | |
| 838 | } | ||
| 839 | // ########################################################################################################### | ||
| 840 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (j.contains("Imu")) |
| 841 | { | ||
| 842 | 8 | Imu::restore(j.at("Imu")); | |
| 843 | } | ||
| 844 | 8 | } | |
| 845 | |||
| 846 | ✗ | 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 | ✗ | auto gpsCycle = static_cast<int32_t>(std::get<double>(line.at(static_cast<size_t>(gpsCycleIter - description.begin())))); | |
| 854 | ✗ | auto gpsWeek = static_cast<int32_t>(std::get<double>(line.at(static_cast<size_t>(gpsWeekIter - description.begin())))); | |
| 855 | ✗ | auto gpsTow = std::get<double>(line.at(static_cast<size_t>(gpsTowIter - description.begin()))); | |
| 856 | ✗ | return { gpsCycle, gpsWeek, gpsTow }; | |
| 857 | } | ||
| 858 | |||
| 859 | ✗ | auto yearUTCIter = std::ranges::find(description, "YearUTC"); | |
| 860 | ✗ | auto monthUTCIter = std::ranges::find(description, "MonthUTC"); | |
| 861 | ✗ | auto dayUTCIter = std::ranges::find(description, "DayUTC"); | |
| 862 | ✗ | auto hourUTCIter = std::ranges::find(description, "HourUTC"); | |
| 863 | ✗ | auto minUTCIter = std::ranges::find(description, "MinUTC"); | |
| 864 | ✗ | auto secUTCIter = std::ranges::find(description, "SecUTC"); | |
| 865 | ✗ | if (yearUTCIter != description.end() && monthUTCIter != description.end() && dayUTCIter != description.end() | |
| 866 | ✗ | && hourUTCIter != description.end() && minUTCIter != description.end() && secUTCIter != description.end()) | |
| 867 | { | ||
| 868 | ✗ | auto yearUTC = static_cast<uint16_t>(std::get<double>(line.at(static_cast<size_t>(yearUTCIter - description.begin())))); | |
| 869 | ✗ | auto monthUTC = static_cast<uint16_t>(std::get<double>(line.at(static_cast<size_t>(monthUTCIter - description.begin())))); | |
| 870 | ✗ | auto dayUTC = static_cast<uint16_t>(std::get<double>(line.at(static_cast<size_t>(dayUTCIter - description.begin())))); | |
| 871 | ✗ | auto hourUTC = static_cast<uint16_t>(std::get<double>(line.at(static_cast<size_t>(hourUTCIter - description.begin())))); | |
| 872 | ✗ | auto minUTC = static_cast<uint16_t>(std::get<double>(line.at(static_cast<size_t>(minUTCIter - description.begin())))); | |
| 873 | ✗ | auto secUTC = std::get<double>(line.at(static_cast<size_t>(secUTCIter - description.begin()))); | |
| 874 | ✗ | return { yearUTC, monthUTC, dayUTC, hourUTC, minUTC, secUTC, UTC }; | |
| 875 | } | ||
| 876 | |||
| 877 | ✗ | LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the time.", nameId()); | |
| 878 | ✗ | return {}; | |
| 879 | } | ||
| 880 | |||
| 881 | ✗ | Eigen::Vector3d NAV::ImuSimulator::e_getPositionFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const // NOLINT(readability-convert-member-functions-to-static) | |
| 882 | { | ||
| 883 | ✗ | auto posXIter = std::ranges::find(description, "Pos ECEF X [m]"); | |
| 884 | ✗ | auto posYIter = std::ranges::find(description, "Pos ECEF Y [m]"); | |
| 885 | ✗ | auto posZIter = std::ranges::find(description, "Pos ECEF Z [m]"); | |
| 886 | ✗ | if (posXIter != description.end() && posYIter != description.end() && posZIter != description.end()) | |
| 887 | { | ||
| 888 | ✗ | auto posX = std::get<double>(line.at(static_cast<size_t>(posXIter - description.begin()))); | |
| 889 | ✗ | auto posY = std::get<double>(line.at(static_cast<size_t>(posYIter - description.begin()))); | |
| 890 | ✗ | auto posZ = std::get<double>(line.at(static_cast<size_t>(posZIter - description.begin()))); | |
| 891 | ✗ | return { posX, posY, posZ }; | |
| 892 | } | ||
| 893 | |||
| 894 | ✗ | auto latIter = std::ranges::find(description, "Latitude [deg]"); | |
| 895 | ✗ | auto lonIter = std::ranges::find(description, "Longitude [deg]"); | |
| 896 | ✗ | auto altIter = std::ranges::find(description, "Altitude [m]"); | |
| 897 | ✗ | if (latIter != description.end() && lonIter != description.end() && altIter != description.end()) | |
| 898 | { | ||
| 899 | ✗ | auto lat = deg2rad(std::get<double>(line.at(static_cast<size_t>(latIter - description.begin())))); | |
| 900 | ✗ | auto lon = deg2rad(std::get<double>(line.at(static_cast<size_t>(lonIter - description.begin())))); | |
| 901 | ✗ | auto alt = std::get<double>(line.at(static_cast<size_t>(altIter - description.begin()))); | |
| 902 | ✗ | return trafo::lla2ecef_WGS84(Eigen::Vector3d(lat, lon, alt)); | |
| 903 | } | ||
| 904 | |||
| 905 | ✗ | LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the position.", nameId()); | |
| 906 | ✗ | return { std::nan(""), std::nan(""), std::nan("") }; | |
| 907 | } | ||
| 908 | |||
| 909 | ✗ | Eigen::Quaterniond NAV::ImuSimulator::n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine& line, const std::vector<std::string>& description) | |
| 910 | { | ||
| 911 | ✗ | auto rollIter = std::ranges::find(description, "Roll [deg]"); | |
| 912 | ✗ | auto pitchIter = std::ranges::find(description, "Pitch [deg]"); | |
| 913 | ✗ | auto yawIter = std::ranges::find(description, "Yaw [deg]"); | |
| 914 | ✗ | if (rollIter != description.end() && pitchIter != description.end() && yawIter != description.end()) | |
| 915 | { | ||
| 916 | ✗ | auto roll = std::get<double>(line.at(static_cast<size_t>(rollIter - description.begin()))); | |
| 917 | ✗ | auto pitch = std::get<double>(line.at(static_cast<size_t>(pitchIter - description.begin()))); | |
| 918 | ✗ | auto yaw = std::get<double>(line.at(static_cast<size_t>(yawIter - description.begin()))); | |
| 919 | ✗ | return trafo::n_Quat_b(deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); | |
| 920 | } | ||
| 921 | |||
| 922 | ✗ | auto quatWIter = std::ranges::find(description, "n_Quat_b w"); | |
| 923 | ✗ | auto quatXIter = std::ranges::find(description, "n_Quat_b x"); | |
| 924 | ✗ | auto quatYIter = std::ranges::find(description, "n_Quat_b y"); | |
| 925 | ✗ | auto quatZIter = std::ranges::find(description, "n_Quat_b z"); | |
| 926 | ✗ | if (quatWIter != description.end() && quatXIter != description.end() && quatYIter != description.end() && quatZIter != description.end()) | |
| 927 | { | ||
| 928 | ✗ | auto w = std::get<double>(line.at(static_cast<size_t>(quatWIter - description.begin()))); | |
| 929 | ✗ | auto x = std::get<double>(line.at(static_cast<size_t>(quatXIter - description.begin()))); | |
| 930 | ✗ | auto y = std::get<double>(line.at(static_cast<size_t>(quatYIter - description.begin()))); | |
| 931 | ✗ | auto z = std::get<double>(line.at(static_cast<size_t>(quatZIter - description.begin()))); | |
| 932 | ✗ | return { w, x, y, z }; | |
| 933 | } | ||
| 934 | |||
| 935 | ✗ | return { std::nan(""), std::nan(""), std::nan(""), std::nan("") }; | |
| 936 | } | ||
| 937 | |||
| 938 | 8 | bool NAV::ImuSimulator::initializeSplines() | |
| 939 | { | ||
| 940 | 8 | std::vector<long double> splineTime; | |
| 941 | |||
| 942 | 1344078 | auto unwrapAngle = [](auto angle, auto prevAngle, auto rangeMax) { | |
| 943 | 1344078 | auto x = angle - prevAngle; | |
| 944 | 1344078 | x = std::fmod(x + rangeMax, 2 * rangeMax); | |
| 945 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1344078 times.
|
1344078 | if (x < 0) |
| 946 | { | ||
| 947 | ✗ | x += 2 * rangeMax; | |
| 948 | } | ||
| 949 | 1344078 | x -= rangeMax; | |
| 950 | |||
| 951 | 1344078 | return prevAngle + x; | |
| 952 | }; | ||
| 953 | |||
| 954 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 2 times.
|
8 | if (_trajectoryType == TrajectoryType::Fixed) |
| 955 | { | ||
| 956 |
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 |
| 957 |
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 |
| 958 |
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 |
| 959 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(0.0); |
| 960 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | splineTime.push_back(_simulationDuration); |
| 961 |
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 |
| 962 |
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 |
| 963 |
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 |
| 964 | |||
| 965 |
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>(); |
| 966 |
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]); |
| 967 |
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]); |
| 968 |
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]); |
| 969 |
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()); |
| 970 |
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()); |
| 971 |
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()); |
| 972 | |||
| 973 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.x.setPoints(splineTime, X); |
| 974 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.y.setPoints(splineTime, Y); |
| 975 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.z.setPoints(splineTime, Z); |
| 976 | |||
| 977 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.roll.setPoints(splineTime, Roll); |
| 978 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.pitch.setPoints(splineTime, Pitch); |
| 979 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _splines.yaw.setPoints(splineTime, Yaw); |
| 980 | 6 | } | |
| 981 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
|
2 | else if (_trajectoryType == TrajectoryType::Linear) |
| 982 | { | ||
| 983 | ✗ | double roll = 0.0; | |
| 984 | ✗ | double pitch = _n_linearTrajectoryStartVelocity.head<2>().norm() > 1e-8 ? calcPitchFromVelocity(_n_linearTrajectoryStartVelocity) : 0; | |
| 985 | ✗ | double yaw = _n_linearTrajectoryStartVelocity.head<2>().norm() > 1e-8 ? calcYawFromVelocity(_n_linearTrajectoryStartVelocity) : 0; | |
| 986 | ✗ | const auto& e_startPosition = _startPosition.e_position; | |
| 987 | |||
| 988 | ✗ | size_t nOverhead = static_cast<size_t>(std::round(1.0 / _splines.sampleInterval)) + 1; | |
| 989 | |||
| 990 | ✗ | splineTime = std::vector<long double>(nOverhead, 0.0); | |
| 991 | ✗ | std::vector<long double> splineX(nOverhead, e_startPosition[0]); | |
| 992 | ✗ | std::vector<long double> splineY(nOverhead, e_startPosition[1]); | |
| 993 | ✗ | std::vector<long double> splineZ(nOverhead, e_startPosition[2]); | |
| 994 | ✗ | std::vector<long double> splineRoll(nOverhead, roll); | |
| 995 | ✗ | std::vector<long double> splinePitch(nOverhead, pitch); | |
| 996 | ✗ | std::vector<long double> splineYaw(nOverhead, yaw); | |
| 997 | |||
| 998 | // @brief Calculates the derivative of the curvilinear position and velocity | ||
| 999 | // @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] | ||
| 1000 | // @param[in] n_acceleration Acceleration in local-navigation frame coordinates [m/s^s] | ||
| 1001 | // @return The curvilinear position and velocity derivatives ∂/∂t [𝜙, λ, h, v_N, v_E, v_D]^T | ||
| 1002 | ✗ | auto f = [](const Eigen::Vector<double, 6>& y, const Eigen::Vector3d& n_acceleration) { | |
| 1003 | ✗ | Eigen::Vector<double, 6> y_dot; | |
| 1004 | ✗ | y_dot << lla_calcTimeDerivativeForPosition(y.tail<3>(), // Velocity with respect to the Earth in local-navigation frame coordinates [m/s] | |
| 1005 | ✗ | y(0), // 𝜙 Latitude in [rad] | |
| 1006 | ✗ | y(2), // h Altitude in [m] | |
| 1007 | ✗ | calcEarthRadius_N(y(0)), // North/South (meridian) earth radius [m] | |
| 1008 | ✗ | calcEarthRadius_E(y(0))), // East/West (prime vertical) earth radius [m] | |
| 1009 | ✗ | n_acceleration; | |
| 1010 | |||
| 1011 | ✗ | return y_dot; | |
| 1012 | }; | ||
| 1013 | |||
| 1014 | ✗ | Eigen::Vector3d lla_lastPosition = _startPosition.latLonAlt(); | |
| 1015 | ✗ | for (size_t i = 2; i <= nOverhead; i++) // Calculate one second backwards | |
| 1016 | { | ||
| 1017 | ✗ | Eigen::Vector<double, 6> y; // [𝜙, λ, h, v_N, v_E, v_D]^T | |
| 1018 | ✗ | y << lla_lastPosition, | |
| 1019 | ✗ | _n_linearTrajectoryStartVelocity; | |
| 1020 | |||
| 1021 | ✗ | y += -_splines.sampleInterval * f(y, Eigen::Vector3d::Zero()); | |
| 1022 | ✗ | lla_lastPosition = y.head<3>(); | |
| 1023 | |||
| 1024 | ✗ | Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_lastPosition); | |
| 1025 | |||
| 1026 | ✗ | splineTime.at(nOverhead - i) = -_splines.sampleInterval * static_cast<double>(i - 1); | |
| 1027 | ✗ | splineX.at(nOverhead - i) = e_position(0); | |
| 1028 | ✗ | splineY.at(nOverhead - i) = e_position(1); | |
| 1029 | ✗ | splineZ.at(nOverhead - i) = e_position(2); | |
| 1030 | } | ||
| 1031 | |||
| 1032 | ✗ | lla_lastPosition = _startPosition.latLonAlt(); | |
| 1033 | ✗ | for (size_t i = 1;; i++) | |
| 1034 | { | ||
| 1035 | ✗ | Eigen::Vector<double, 6> y; // [𝜙, λ, h, v_N, v_E, v_D]^T | |
| 1036 | ✗ | y << lla_lastPosition, | |
| 1037 | ✗ | _n_linearTrajectoryStartVelocity; | |
| 1038 | |||
| 1039 | ✗ | y += _splines.sampleInterval * f(y, Eigen::Vector3d::Zero()); | |
| 1040 | ✗ | lla_lastPosition = y.head<3>(); | |
| 1041 | |||
| 1042 | ✗ | auto e_position = trafo::lla2ecef_WGS84(lla_lastPosition); | |
| 1043 | |||
| 1044 | ✗ | auto simTime = _splines.sampleInterval * static_cast<double>(i); | |
| 1045 | ✗ | splineTime.push_back(simTime); | |
| 1046 | ✗ | splineX.push_back(e_position(0)); | |
| 1047 | ✗ | splineY.push_back(e_position(1)); | |
| 1048 | ✗ | splineZ.push_back(e_position(2)); | |
| 1049 | ✗ | splineRoll.push_back(roll); | |
| 1050 | ✗ | splinePitch.push_back(pitch); | |
| 1051 | ✗ | splineYaw.push_back(yaw); | |
| 1052 | |||
| 1053 | ✗ | if (_simulationStopCondition == StopCondition::Duration | |
| 1054 | ✗ | && simTime > _simulationDuration + 1.0) | |
| 1055 | { | ||
| 1056 | ✗ | break; | |
| 1057 | } | ||
| 1058 | ✗ | if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses) | |
| 1059 | { | ||
| 1060 | ✗ | auto horizontalDistance = calcGeographicalDistance(_startPosition.latitude(), _startPosition.longitude(), | |
| 1061 | ✗ | lla_lastPosition(0), lla_lastPosition(1)); | |
| 1062 | ✗ | auto distance = std::sqrt(std::pow(horizontalDistance, 2) + std::pow(_startPosition.altitude() - lla_lastPosition(2), 2)) | |
| 1063 | ✗ | - _n_linearTrajectoryStartVelocity.norm() * 1.0; | |
| 1064 | ✗ | if (distance > _linearTrajectoryDistanceForStop) | |
| 1065 | { | ||
| 1066 | ✗ | break; | |
| 1067 | } | ||
| 1068 | } | ||
| 1069 | ✗ | } | |
| 1070 | |||
| 1071 | ✗ | _splines.x.setPoints(splineTime, splineX); | |
| 1072 | ✗ | _splines.y.setPoints(splineTime, splineY); | |
| 1073 | ✗ | _splines.z.setPoints(splineTime, splineZ); | |
| 1074 | |||
| 1075 | ✗ | _splines.roll.setPoints(splineTime, splineRoll); | |
| 1076 | ✗ | _splines.pitch.setPoints(splineTime, splinePitch); | |
| 1077 | ✗ | _splines.yaw.setPoints(splineTime, splineYaw); | |
| 1078 | ✗ | } | |
| 1079 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | else if (_trajectoryType == TrajectoryType::Circular) |
| 1080 | { | ||
| 1081 | 1 | double simDuration{}; | |
| 1082 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (_simulationStopCondition == StopCondition::Duration) |
| 1083 | { | ||
| 1084 | ✗ | simDuration = _simulationDuration; | |
| 1085 | } | ||
| 1086 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | else if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses) |
| 1087 | { | ||
| 1088 | 1 | double omega = _trajectoryHorizontalSpeed / _trajectoryRadius; | |
| 1089 | 1 | simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega; | |
| 1090 | } | ||
| 1091 | |||
| 1092 | 1 | constexpr double OFFSET = 1.0; // [s] | |
| 1093 | |||
| 1094 |
2/2✓ Branch 0 taken 335 times.
✓ Branch 1 taken 1 times.
|
336 | for (size_t i = 0; i <= static_cast<size_t>(std::round((simDuration + 2.0 * OFFSET) / _splines.sampleInterval)); i++) |
| 1095 | { | ||
| 1096 |
1/2✓ Branch 1 taken 335 times.
✗ Branch 2 not taken.
|
335 | splineTime.push_back(_splines.sampleInterval * static_cast<double>(i) - OFFSET); |
| 1097 | } | ||
| 1098 | LOG_DATA("{}: Sim Time [{:.3f}, {:.3f}] with dt = {:.3f} (simDuration = {:.3f})", nameId(), | ||
| 1099 | static_cast<double>(splineTime.front()), static_cast<double>(splineTime.back()), static_cast<double>(splineTime.at(1) - splineTime.at(0)), simDuration); | ||
| 1100 | |||
| 1101 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineX(splineTime.size()); |
| 1102 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineY(splineTime.size()); |
| 1103 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::vector<long double> splineZ(splineTime.size()); |
| 1104 | |||
| 1105 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Vector3d e_origin = _startPosition.e_position; |
| 1106 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Vector3d lla_origin = _startPosition.latLonAlt(); |
| 1107 | |||
| 1108 |
3/6✓ 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.
|
1 | Eigen::Quaterniond e_quatCenter_n = trafo::e_Quat_n(lla_origin(0), lla_origin(1)); |
| 1109 | |||
| 1110 |
2/2✓ Branch 1 taken 335 times.
✓ Branch 2 taken 1 times.
|
336 | for (uint64_t i = 0; i < splineTime.size(); i++) |
| 1111 | { | ||
| 1112 | 335 | auto phi = _trajectoryHorizontalSpeed * static_cast<double>(splineTime[i]) / _trajectoryRadius; // Angle of the current point on the circle | |
| 1113 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 335 times.
|
335 | phi *= _trajectoryDirection == Direction::CW ? -1 : 1; |
| 1114 | 335 | phi += _trajectoryRotationAngle; | |
| 1115 | // LOG_DATA("{}: [t={:.3f}] phi = {:.3f}°", nameId(), static_cast<double>(splineTime.at(i)), rad2deg(phi)); | ||
| 1116 | |||
| 1117 | ✗ | Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::sin(phi) * (1 + _circularHarmonicAmplitudeFactor * std::sin(phi * static_cast<double>(_circularHarmonicFrequency))), // [m] | |
| 1118 | ✗ | _trajectoryRadius * std::cos(phi) * (1 + _circularHarmonicAmplitudeFactor * std::sin(phi * static_cast<double>(_circularHarmonicFrequency))), // [m] | |
| 1119 |
1/2✓ Branch 2 taken 335 times.
✗ Branch 3 not taken.
|
335 | -_trajectoryVerticalSpeed * static_cast<double>(splineTime[i]) }; // [m] |
| 1120 | |||
| 1121 |
1/2✓ Branch 1 taken 335 times.
✗ Branch 2 not taken.
|
335 | Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition; |
| 1122 | |||
| 1123 |
2/4✓ Branch 1 taken 335 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335 times.
✗ Branch 5 not taken.
|
335 | Eigen::Vector3d e_position = e_origin + e_relativePosition; |
| 1124 | |||
| 1125 |
1/2✓ Branch 1 taken 335 times.
✗ Branch 2 not taken.
|
335 | splineX[i] = e_position[0]; |
| 1126 |
1/2✓ Branch 1 taken 335 times.
✗ Branch 2 not taken.
|
335 | splineY[i] = e_position[1]; |
| 1127 |
1/2✓ Branch 1 taken 335 times.
✗ Branch 2 not taken.
|
335 | splineZ[i] = e_position[2]; |
| 1128 | } | ||
| 1129 | |||
| 1130 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.x.setPoints(splineTime, splineX); |
| 1131 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.y.setPoints(splineTime, splineY); |
| 1132 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.z.setPoints(splineTime, splineZ); |
| 1133 | 1 | } | |
| 1134 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | else if (_trajectoryType == TrajectoryType::RoseFigure) |
| 1135 | { | ||
| 1136 | // Formulas and notation for the rose figure from https://en.wikipedia.org/wiki/Rose_(mathematics) | ||
| 1137 | |||
| 1138 | // Adjusting the integration bounds needs a factor * PI | ||
| 1139 | // | d \ n | 1 | 2 | 3 | 4 | 5 | 6 | 7 | | ||
| 1140 | // | ------ | --- | --- | --- | --- | --- | --- | --- | | ||
| 1141 | // | 1 | 1 | 2 | 1 | 2 | 1 | 2 | 1 | | ||
| 1142 | // | 2 | 4 | 1 | 4 | 2 | 4 | 1 | 4 | | ||
| 1143 | // | 3 | 3 | 6 | 1 | 6 | 3 | 2 | 3 | | ||
| 1144 | // | 4 | 8 | 4 | 8 | 1 | 8 | 4 | 8 | | ||
| 1145 | // | 5 | 5 | 10 | 5 | 10 | 1 | 10 | 5 | | ||
| 1146 | // | 6 | 12 | 3 | 4 | 6 | 12 | 1 | 12 | | ||
| 1147 | // | 7 | 7 | 14 | 7 | 14 | 7 | 14 | 1 | | ||
| 1148 | // | 8 | 16 | 8 | 16 | 4 | 16 | 8 | 16 | | ||
| 1149 | // | 9 | 9 | 18 | 3 | 18 | 9 | 6 | 9 | | ||
| 1150 | |||
| 1151 | 1 | int n = _rosePetNum; | |
| 1152 | 1 | int d = _rosePetDenom; | |
| 1153 | |||
| 1154 |
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 ) |
| 1155 | { | ||
| 1156 |
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) |
| 1157 | { | ||
| 1158 | ✗ | n /= i; | |
| 1159 | ✗ | d /= i; | |
| 1160 | ✗ | i--; | |
| 1161 | } | ||
| 1162 | } | ||
| 1163 | |||
| 1164 | 2 | auto isOdd = [](auto a) { return static_cast<int>(a) % 2 != 0; }; | |
| 1165 | ✗ | auto isEven = [](auto a) { return static_cast<int>(a) % 2 == 0; }; | |
| 1166 | |||
| 1167 | 1 | double integrationFactor = 0.0; | |
| 1168 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (isOdd(d)) |
| 1169 | { | ||
| 1170 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (isOdd(n)) { integrationFactor = static_cast<double>(d); } |
| 1171 | 1 | else { integrationFactor = 2.0 * static_cast<double>(d); } | |
| 1172 | } | ||
| 1173 | else // if (isEven(d)) | ||
| 1174 | { | ||
| 1175 | ✗ | if (isEven(n)) { integrationFactor = static_cast<double>(d); } | |
| 1176 | ✗ | else { integrationFactor = 2.0 * static_cast<double>(d); } | |
| 1177 | } | ||
| 1178 | |||
| 1179 | 1 | auto nVirtPoints = static_cast<size_t>(1.0 / (_roseStepLengthMax / 50.0)); | |
| 1180 |
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 |
| 1181 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineX(splineTime.size()); |
| 1182 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineY(splineTime.size()); |
| 1183 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::vector<long double> splineZ(splineTime.size()); |
| 1184 | |||
| 1185 |
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()); |
| 1186 | |||
| 1187 |
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)); |
| 1188 | |||
| 1189 | 1 | double lengthOld = -_roseStepLengthMax / 2.0; // n-1 length | |
| 1190 | 1 | double dPhi = 0.001; // Angle step size | |
| 1191 | 1 | double maxPhi = std::numeric_limits<double>::infinity(); // Interval for integration depending on selected stop criteria | |
| 1192 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses) |
| 1193 | { | ||
| 1194 | 1 | maxPhi = _roseTrajectoryCountForStop * integrationFactor * M_PI; | |
| 1195 | } | ||
| 1196 | |||
| 1197 | // k = n/d | ||
| 1198 | 1 | double roseK = static_cast<double>(_rosePetNum) / static_cast<double>(_rosePetDenom); | |
| 1199 | |||
| 1200 | 1 | _roseSimDuration = 0.0; | |
| 1201 | |||
| 1202 | // We cannot input negative values or zero | ||
| 1203 |
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) |
| 1204 | { | ||
| 1205 |
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)); |
| 1206 | 1342086 | double dL = length - lengthOld; | |
| 1207 | |||
| 1208 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1342080 times.
|
1342086 | if (dL > _roseStepLengthMax) |
| 1209 | { | ||
| 1210 | 6 | phi -= dPhi; | |
| 1211 | 6 | dPhi /= 2.0; | |
| 1212 | 7 | continue; | |
| 1213 | } | ||
| 1214 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1342079 times.
|
1342080 | if (dL < _roseStepLengthMax / 3.0) // Avoid also too small steps |
| 1215 | { | ||
| 1216 | 1 | phi -= dPhi; | |
| 1217 | 1 | dPhi *= 1.5; | |
| 1218 | 1 | continue; | |
| 1219 | } | ||
| 1220 | 1342079 | lengthOld = length; | |
| 1221 | |||
| 1222 | 1342079 | double time = length / _trajectoryHorizontalSpeed; | |
| 1223 |
1/2✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
|
1342079 | splineTime.push_back(time); |
| 1224 |
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] |
| 1225 |
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] |
| 1226 |
1/2✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
|
1342079 | -_trajectoryVerticalSpeed * time }; // [m] |
| 1227 | |||
| 1228 |
1/2✓ Branch 1 taken 1342079 times.
✗ Branch 2 not taken.
|
1342079 | Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition; |
| 1229 |
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; |
| 1230 | |||
| 1231 | // LOG_DATA("{}: t={: 8.3f}s | l={:8.6}m | phi={:6.3f}", nameId(), time, length, rad2deg(phi)); | ||
| 1232 | |||
| 1233 |
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]); |
| 1234 |
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]); |
| 1235 |
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]); |
| 1236 | |||
| 1237 |
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) |
| 1238 | { | ||
| 1239 | LOG_TRACE("{}: Rose figure simulation duration: {:8.6}s | l={:8.6}m", nameId(), time, length); | ||
| 1240 | 2 | _roseSimDuration = time; | |
| 1241 | } | ||
| 1242 |
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) |
| 1243 | { | ||
| 1244 | ✗ | _roseSimDuration = _simulationDuration; | |
| 1245 | ✗ | maxPhi = phi; | |
| 1246 | } | ||
| 1247 | } | ||
| 1248 | |||
| 1249 | 1 | maxPhi = integrationFactor * M_PI + 1e-15; // For exactly 2*pi the elliptical integral is failing. Bug in the function. | |
| 1250 | // LOG_DATA("maxPhi {:6.2f}", rad2deg(maxPhi)); | ||
| 1251 |
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)); |
| 1252 | // LOG_DATA("endLength {:8.6}m", endLength); | ||
| 1253 |
2/2✓ Branch 0 taken 1666 times.
✓ Branch 1 taken 1 times.
|
1667 | for (size_t i = 0; i < nVirtPoints; i++) |
| 1254 | { | ||
| 1255 | 1666 | double phi = maxPhi - static_cast<double>(i) * dPhi; | |
| 1256 |
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)); |
| 1257 | 1666 | double time = (length - endLength) / _trajectoryHorizontalSpeed; | |
| 1258 | 1666 | splineTime[nVirtPoints - i - 1] = time; | |
| 1259 | |||
| 1260 |
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] |
| 1261 |
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] |
| 1262 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | -_trajectoryVerticalSpeed * time }; // [m] |
| 1263 | |||
| 1264 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition; |
| 1265 |
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; |
| 1266 | |||
| 1267 | // LOG_DATA("{}: t={: 8.3f}s | l={:8.6}m | phi={:6.3f}", nameId(), time, length, rad2deg(phi)); | ||
| 1268 | |||
| 1269 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | splineX[nVirtPoints - i - 1] = e_position[0]; |
| 1270 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | splineY[nVirtPoints - i - 1] = e_position[1]; |
| 1271 |
1/2✓ Branch 1 taken 1666 times.
✗ Branch 2 not taken.
|
1666 | splineZ[nVirtPoints - i - 1] = e_position[2]; |
| 1272 | } | ||
| 1273 | |||
| 1274 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.x.setPoints(splineTime, splineX); |
| 1275 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.y.setPoints(splineTime, splineY); |
| 1276 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _splines.z.setPoints(splineTime, splineZ); |
| 1277 | 1 | } | |
| 1278 | ✗ | else if (_trajectoryType == TrajectoryType::Csv) | |
| 1279 | { | ||
| 1280 | ✗ | if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV); | |
| 1281 | ✗ | csvData && csvData->v->lines.size() >= 2) | |
| 1282 | { | ||
| 1283 | ✗ | _startTime = getTimeFromCsvLine(csvData->v->lines.front(), csvData->v->description); | |
| 1284 | ✗ | if (_startTime.empty()) { return false; } | |
| 1285 | |||
| 1286 | ✗ | constexpr size_t nVirtPoints = 0; | |
| 1287 | ✗ | splineTime.resize(nVirtPoints); // Preallocate points to make the spline start at the right point | |
| 1288 | ✗ | std::vector<long double> splineX(splineTime.size()); | |
| 1289 | ✗ | std::vector<long double> splineY(splineTime.size()); | |
| 1290 | ✗ | std::vector<long double> splineZ(splineTime.size()); | |
| 1291 | ✗ | std::vector<long double> splineRoll(splineTime.size()); | |
| 1292 | ✗ | std::vector<long double> splinePitch(splineTime.size()); | |
| 1293 | ✗ | std::vector<long double> splineYaw(splineTime.size()); | |
| 1294 | |||
| 1295 | ✗ | for (size_t i = 0; i < csvData->v->lines.size(); i++) | |
| 1296 | { | ||
| 1297 | ✗ | InsTime insTime = getTimeFromCsvLine(csvData->v->lines[i], csvData->v->description); | |
| 1298 | ✗ | if (insTime.empty()) { return false; } | |
| 1299 | // LOG_DATA("{}: Time {}", nameId(), insTime); | ||
| 1300 | ✗ | auto time = static_cast<double>((insTime - _startTime).count()); | |
| 1301 | |||
| 1302 | ✗ | Eigen::Vector3d e_pos = e_getPositionFromCsvLine(csvData->v->lines[i], csvData->v->description); | |
| 1303 | ✗ | if (std::isnan(e_pos.x())) { return false; } | |
| 1304 | // LOG_DATA("{}: e_pos {}", nameId(), e_pos); | ||
| 1305 | |||
| 1306 | ✗ | Eigen::Quaterniond n_Quat_b = n_getAttitudeQuaternionFromCsvLine_b(csvData->v->lines[i], csvData->v->description); | |
| 1307 | ✗ | if (std::isnan(n_Quat_b.w())) | |
| 1308 | { | ||
| 1309 | // TODO: Calculate with rotation minimizing frame instead of returning false | ||
| 1310 | ✗ | return false; | |
| 1311 | } | ||
| 1312 | // LOG_DATA("{}: n_Quat_b {}", nameId(), n_Quat_b); | ||
| 1313 | |||
| 1314 | ✗ | splineTime.push_back(time); | |
| 1315 | ✗ | splineX.push_back(e_pos.x()); | |
| 1316 | ✗ | splineY.push_back(e_pos.y()); | |
| 1317 | ✗ | splineZ.push_back(e_pos.z()); | |
| 1318 | |||
| 1319 | ✗ | auto rpy = trafo::quat2eulerZYX(n_Quat_b); | |
| 1320 | // LOG_DATA("{}: RPY {} [deg] (from CSV)", nameId(), rad2deg(rpy).transpose()); | ||
| 1321 | ✗ | splineRoll.push_back(i > 0 ? unwrapAngle(rpy(0), splineRoll.back(), M_PI) : rpy(0)); | |
| 1322 | ✗ | splinePitch.push_back(i > 0 ? unwrapAngle(rpy(1), splinePitch.back(), M_PI_2) : rpy(1)); | |
| 1323 | ✗ | splineYaw.push_back(i > 0 ? unwrapAngle(rpy(2), splineYaw.back(), M_PI) : rpy(2)); | |
| 1324 | // LOG_DATA("{}: R {}, P {}, Y {} [deg] (in Spline)", nameId(), rad2deg(splineRoll.back()), rad2deg(splinePitch.back()), rad2deg(splineYaw.back())); | ||
| 1325 | } | ||
| 1326 | ✗ | _csvDuration = static_cast<double>(splineTime.back()); | |
| 1327 | |||
| 1328 | ✗ | auto dt = static_cast<double>(splineTime[nVirtPoints + 1] - splineTime[nVirtPoints]); | |
| 1329 | ✗ | for (size_t i = 0; i < nVirtPoints; i++) | |
| 1330 | { | ||
| 1331 | double h = 0.001; | ||
| 1332 | splineTime[nVirtPoints - i - 1] = splineTime[nVirtPoints - i] - h; | ||
| 1333 | splineX[nVirtPoints - i - 1] = splineX[nVirtPoints - i] - h * (splineX[nVirtPoints + 1] - splineX[nVirtPoints]) / dt; | ||
| 1334 | splineY[nVirtPoints - i - 1] = splineY[nVirtPoints - i] - h * (splineY[nVirtPoints + 1] - splineY[nVirtPoints]) / dt; | ||
| 1335 | splineZ[nVirtPoints - i - 1] = splineZ[nVirtPoints - i] - h * (splineZ[nVirtPoints + 1] - splineZ[nVirtPoints]) / dt; | ||
| 1336 | splineRoll[nVirtPoints - i - 1] = splineRoll[nVirtPoints - i] - h * (splineRoll[nVirtPoints + 1] - splineRoll[nVirtPoints]) / dt; | ||
| 1337 | splinePitch[nVirtPoints - i - 1] = splinePitch[nVirtPoints - i] - h * (splinePitch[nVirtPoints + 1] - splinePitch[nVirtPoints]) / dt; | ||
| 1338 | splineYaw[nVirtPoints - i - 1] = splineYaw[nVirtPoints - i] - h * (splineYaw[nVirtPoints + 1] - splineYaw[nVirtPoints]) / dt; | ||
| 1339 | splineTime.push_back(splineTime[splineX.size() - 1] + h); | ||
| 1340 | splineX.push_back(splineX[splineX.size() - 1] + h * (splineX[splineX.size() - 1] - splineX[splineX.size() - 2]) / dt); | ||
| 1341 | splineY.push_back(splineY[splineY.size() - 1] + h * (splineY[splineY.size() - 1] - splineY[splineY.size() - 2]) / dt); | ||
| 1342 | splineZ.push_back(splineZ[splineZ.size() - 1] + h * (splineZ[splineZ.size() - 1] - splineZ[splineZ.size() - 2]) / dt); | ||
| 1343 | splineRoll.push_back(splineRoll[splineRoll.size() - 1] + h * (splineRoll[splineRoll.size() - 1] - splineRoll[splineRoll.size() - 2]) / dt); | ||
| 1344 | splinePitch.push_back(splinePitch[splinePitch.size() - 1] + h * (splinePitch[splinePitch.size() - 1] - splinePitch[splinePitch.size() - 2]) / dt); | ||
| 1345 | splineYaw.push_back(splineYaw[splineYaw.size() - 1] + h * (splineYaw[splineYaw.size() - 1] - splineYaw[splineYaw.size() - 2]) / dt); | ||
| 1346 | } | ||
| 1347 | |||
| 1348 | ✗ | _splines.x.setPoints(splineTime, splineX); | |
| 1349 | ✗ | _splines.y.setPoints(splineTime, splineY); | |
| 1350 | ✗ | _splines.z.setPoints(splineTime, splineZ); | |
| 1351 | |||
| 1352 | ✗ | _splines.roll.setPoints(splineTime, splineRoll); | |
| 1353 | ✗ | _splines.pitch.setPoints(splineTime, splinePitch); | |
| 1354 | ✗ | _splines.yaw.setPoints(splineTime, splineYaw); | |
| 1355 | ✗ | } | |
| 1356 | else | ||
| 1357 | { | ||
| 1358 | ✗ | LOG_ERROR("{}: Can't calculate the data without a connected CSV file with at least two datasets", nameId()); | |
| 1359 | ✗ | return false; | |
| 1360 | ✗ | } | |
| 1361 | } | ||
| 1362 | |||
| 1363 |
4/4✓ Branch 0 taken 7 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 6 times.
|
8 | if (_trajectoryType == TrajectoryType::Circular || _trajectoryType == TrajectoryType::RoseFigure) |
| 1364 | { | ||
| 1365 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | std::vector<long double> splineRoll(splineTime.size()); |
| 1366 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | std::vector<long double> splinePitch(splineTime.size()); |
| 1367 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | std::vector<long double> splineYaw(splineTime.size()); |
| 1368 | |||
| 1369 |
2/2✓ Branch 1 taken 1344080 times.
✓ Branch 2 taken 2 times.
|
1344082 | for (uint64_t i = 0; i < splineTime.size(); i++) |
| 1370 | { | ||
| 1371 |
1/2✓ Branch 2 taken 1344080 times.
✗ Branch 3 not taken.
|
1344080 | Eigen::Vector3d e_pos{ static_cast<double>(_splines.x(splineTime[i])), |
| 1372 |
1/2✓ Branch 2 taken 1344080 times.
✗ Branch 3 not taken.
|
1344080 | static_cast<double>(_splines.y(splineTime[i])), |
| 1373 |
2/4✓ Branch 2 taken 1344080 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1344080 times.
✗ Branch 6 not taken.
|
1344080 | static_cast<double>(_splines.z(splineTime[i])) }; |
| 1374 |
1/2✓ Branch 2 taken 1344080 times.
✗ Branch 3 not taken.
|
1344080 | Eigen::Vector3d e_vel{ static_cast<double>(_splines.x.derivative(1, splineTime[i])), |
| 1375 |
1/2✓ Branch 2 taken 1344080 times.
✗ Branch 3 not taken.
|
1344080 | static_cast<double>(_splines.y.derivative(1, splineTime[i])), |
| 1376 |
2/4✓ Branch 2 taken 1344080 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1344080 times.
✗ Branch 6 not taken.
|
1344080 | static_cast<double>(_splines.z.derivative(1, splineTime[i])) }; |
| 1377 | |||
| 1378 |
1/2✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
|
1344080 | Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(e_pos); |
| 1379 |
4/8✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1344080 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1344080 times.
✗ Branch 11 not taken.
|
1344080 | Eigen::Vector3d n_velocity = trafo::n_Quat_e(lla_position(0), lla_position(1)) * e_vel; |
| 1380 | |||
| 1381 |
4/8✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1344080 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1344080 times.
✗ Branch 11 not taken.
|
1344080 | Eigen::Vector3d e_normalVectorCenterCircle{ std::cos(_startPosition.latLonAlt()(0)) * std::cos(_startPosition.latLonAlt()(1)), |
| 1382 |
4/8✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1344080 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1344080 times.
✗ Branch 11 not taken.
|
1344080 | std::cos(_startPosition.latLonAlt()(0)) * std::sin(_startPosition.latLonAlt()(1)), |
| 1383 |
3/6✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1344080 times.
✗ Branch 8 not taken.
|
1344080 | std::sin(_startPosition.latLonAlt()(0)) }; |
| 1384 | |||
| 1385 |
2/4✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
|
1344080 | Eigen::Vector3d e_normalVectorCurrentPosition{ std::cos(lla_position(0)) * std::cos(lla_position(1)), |
| 1386 |
2/4✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
|
1344080 | std::cos(lla_position(0)) * std::sin(lla_position(1)), |
| 1387 |
2/4✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
|
1344080 | std::sin(lla_position(0)) }; |
| 1388 | |||
| 1389 |
1/2✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
|
1344080 | auto yaw = calcYawFromVelocity(n_velocity); |
| 1390 | |||
| 1391 |
3/4✓ Branch 0 taken 1344078 times.
✓ Branch 1 taken 2 times.
✓ Branch 4 taken 1344078 times.
✗ Branch 5 not taken.
|
1344080 | splineYaw[i] = i > 0 ? unwrapAngle(yaw, splineYaw[i - 1], M_PI) : yaw; |
| 1392 | 1344080 | splineRoll[i] = 0.0; | |
| 1393 |
4/8✓ Branch 1 taken 1344080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344080 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1344080 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1344080 times.
✗ Branch 10 not taken.
|
1344080 | splinePitch[i] = n_velocity.head<2>().norm() > 1e-8 ? calcPitchFromVelocity(n_velocity) : 0; |
| 1394 | // LOG_DATA("{}: [t={:.3f}] yaw = {:.3f}°", nameId(), static_cast<double>(splineTime.at(i)), rad2deg(splineYaw[i])); | ||
| 1395 | } | ||
| 1396 | |||
| 1397 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | _splines.roll.setPoints(splineTime, splineRoll); |
| 1398 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | _splines.pitch.setPoints(splineTime, splinePitch); |
| 1399 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | _splines.yaw.setPoints(splineTime, splineYaw); |
| 1400 | 2 | } | |
| 1401 | |||
| 1402 | 8 | return true; | |
| 1403 | 8 | } | |
| 1404 | |||
| 1405 | 8 | bool NAV::ImuSimulator::initialize() | |
| 1406 | { | ||
| 1407 | LOG_TRACE("{}: called", nameId()); | ||
| 1408 | |||
| 1409 | 8 | return initializeSplines(); | |
| 1410 | } | ||
| 1411 | |||
| 1412 | 8 | void NAV::ImuSimulator::deinitialize() | |
| 1413 | { | ||
| 1414 | LOG_TRACE("{}: called", nameId()); | ||
| 1415 | 8 | } | |
| 1416 | |||
| 1417 | 16 | bool NAV::ImuSimulator::resetNode() | |
| 1418 | { | ||
| 1419 | LOG_TRACE("{}: called", nameId()); | ||
| 1420 | |||
| 1421 | 16 | _imuInternalUpdateCnt = 0; | |
| 1422 | 16 | _imuUpdateCnt = 0; | |
| 1423 | 16 | _gnssUpdateCnt = 0; | |
| 1424 | |||
| 1425 | 16 | _imuLastUpdateTime = 0.0; | |
| 1426 | 16 | _gnssLastUpdateTime = 0.0; | |
| 1427 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | _lla_imuLastLinearPosition = _startPosition.latLonAlt(); |
| 1428 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | _lla_gnssLastLinearPosition = _startPosition.latLonAlt(); |
| 1429 | |||
| 1430 |
3/6✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
|
16 | _p_lastImuAccelerationMeas = Eigen::Vector3d::Ones() * std::nan(""); |
| 1431 |
3/6✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
|
16 | _p_lastImuAngularRateMeas = Eigen::Vector3d::Ones() * std::nan(""); |
| 1432 | |||
| 1433 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
16 | if (_trajectoryType == TrajectoryType::Csv) |
| 1434 | { | ||
| 1435 | ✗ | if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV); | |
| 1436 | ✗ | csvData && !csvData->v->lines.empty()) | |
| 1437 | { | ||
| 1438 | ✗ | _startTime = getTimeFromCsvLine(csvData->v->lines.front(), csvData->v->description); | |
| 1439 | ✗ | if (_startTime.empty()) | |
| 1440 | { | ||
| 1441 | ✗ | return false; | |
| 1442 | } | ||
| 1443 | ✗ | LOG_DEBUG("{}: Start Time set to {}", nameId(), _startTime); | |
| 1444 | } | ||
| 1445 | else | ||
| 1446 | { | ||
| 1447 | ✗ | LOG_ERROR("{}: Can't reset the ImuSimulator without a connected CSV file", nameId()); | |
| 1448 | ✗ | return false; | |
| 1449 | ✗ | } | |
| 1450 | } | ||
| 1451 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
16 | else if (_startTimeSource == StartTimeSource::CurrentComputerTime) |
| 1452 | { | ||
| 1453 | ✗ | std::time_t t = std::time(nullptr); | |
| 1454 | ✗ | std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe) | |
| 1455 | |||
| 1456 | ✗ | _startTime = InsTime{ static_cast<uint16_t>(now->tm_year + 1900), static_cast<uint16_t>(now->tm_mon), static_cast<uint16_t>(now->tm_mday), | |
| 1457 | ✗ | static_cast<uint16_t>(now->tm_hour), static_cast<uint16_t>(now->tm_min), static_cast<long double>(now->tm_sec) }; | |
| 1458 | ✗ | LOG_DEBUG("{}: Start Time set to {}", nameId(), _startTime); | |
| 1459 | } | ||
| 1460 | |||
| 1461 | 16 | return true; | |
| 1462 | } | ||
| 1463 | |||
| 1464 | 120451 | bool NAV::ImuSimulator::checkStopCondition(double time, const Eigen::Vector3d& lla_position) | |
| 1465 | { | ||
| 1466 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 120451 times.
|
120451 | if (_trajectoryType == TrajectoryType::Csv) |
| 1467 | { | ||
| 1468 | ✗ | return time > _csvDuration; | |
| 1469 | } | ||
| 1470 |
2/2✓ Branch 0 taken 100036 times.
✓ Branch 1 taken 20415 times.
|
120451 | if (_simulationStopCondition == StopCondition::Duration |
| 1471 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 100036 times.
|
100036 | || _trajectoryType == TrajectoryType::Fixed) |
| 1472 | { | ||
| 1473 | 20415 | return time > _simulationDuration; | |
| 1474 | } | ||
| 1475 |
1/2✓ Branch 0 taken 100036 times.
✗ Branch 1 not taken.
|
100036 | if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses) |
| 1476 | { | ||
| 1477 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 100036 times.
|
100036 | if (_trajectoryType == TrajectoryType::Linear) |
| 1478 | { | ||
| 1479 | ✗ | auto horizontalDistance = calcGeographicalDistance(_startPosition.latitude(), _startPosition.longitude(), | |
| 1480 | ✗ | lla_position(0), lla_position(1)); | |
| 1481 | ✗ | auto distance = std::sqrt(std::pow(horizontalDistance, 2) + std::pow(_startPosition.altitude() - lla_position(2), 2)); | |
| 1482 | ✗ | return distance > _linearTrajectoryDistanceForStop; | |
| 1483 | } | ||
| 1484 |
2/2✓ Branch 0 taken 3146 times.
✓ Branch 1 taken 96890 times.
|
100036 | if (_trajectoryType == TrajectoryType::Circular) |
| 1485 | { | ||
| 1486 | 3146 | double omega = _trajectoryHorizontalSpeed / _trajectoryRadius; | |
| 1487 | 3146 | double simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega; | |
| 1488 | 3146 | return time > simDuration; | |
| 1489 | } | ||
| 1490 |
1/2✓ Branch 0 taken 96890 times.
✗ Branch 1 not taken.
|
96890 | if (_trajectoryType == TrajectoryType::RoseFigure) |
| 1491 | { | ||
| 1492 | 96890 | return time > _roseSimDuration; | |
| 1493 | } | ||
| 1494 | } | ||
| 1495 | ✗ | return false; | |
| 1496 | } | ||
| 1497 | |||
| 1498 | 70432 | std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollImuObs(size_t /* pinIdx */, bool peek) | |
| 1499 | { | ||
| 1500 | 70432 | double imuUpdateTime = static_cast<double>(_imuUpdateCnt) / _imuFrequency; | |
| 1501 | |||
| 1502 | // -------------------------------------------------- Check if a stop condition is met ----------------------------------------------------- | ||
| 1503 |
4/6✓ Branch 1 taken 70432 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70433 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8 times.
✓ Branch 7 taken 70425 times.
|
70432 | if (checkStopCondition(imuUpdateTime, lla_calcPosition(imuUpdateTime))) |
| 1504 | { | ||
| 1505 | 8 | return nullptr; | |
| 1506 | } | ||
| 1507 | // ------------------------------------- Early return in case of peeking to avoid heavy calculations --------------------------------------- | ||
| 1508 |
2/2✓ Branch 0 taken 35206 times.
✓ Branch 1 taken 35219 times.
|
70425 | if (peek) |
| 1509 | { | ||
| 1510 |
1/2✓ Branch 1 taken 35209 times.
✗ Branch 2 not taken.
|
35206 | auto obs = std::make_shared<NodeData>(); |
| 1511 |
2/4✓ Branch 2 taken 35212 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35213 times.
✗ Branch 6 not taken.
|
35209 | obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime); |
| 1512 | 35204 | return obs; | |
| 1513 | 35202 | } | |
| 1514 | |||
| 1515 |
1/2✓ Branch 1 taken 35211 times.
✗ Branch 2 not taken.
|
35219 | auto obs = std::make_shared<ImuObsSimulated>(_imuPos); |
| 1516 | 35211 | obs->timeSinceStartup = static_cast<uint64_t>(imuUpdateTime * 1e9); | |
| 1517 |
2/4✓ Branch 2 taken 35204 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35211 times.
✗ Branch 6 not taken.
|
35204 | obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime); |
| 1518 | LOG_DATA("{}: Simulated IMU data for time [{}] (_imuUpdateCnt {}, _imuInternalUpdateCnt {})", nameId(), obs->insTime.toYMDHMS(), | ||
| 1519 | _imuUpdateCnt, _imuInternalUpdateCnt); | ||
| 1520 | |||
| 1521 | 35213 | double dTime = 0.0; | |
| 1522 |
2/4✓ Branch 1 taken 35203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35208 times.
✗ Branch 5 not taken.
|
35213 | Eigen::Vector3d p_deltaVel = Eigen::Vector3d::Zero(); |
| 1523 |
2/4✓ Branch 1 taken 35205 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35209 times.
✗ Branch 5 not taken.
|
35208 | Eigen::Vector3d p_deltaTheta = Eigen::Vector3d::Zero(); |
| 1524 | |||
| 1525 | 35209 | double imuInternalUpdateTime = 0.0; | |
| 1526 | do { | ||
| 1527 | 2866616 | imuInternalUpdateTime = static_cast<double>(_imuInternalUpdateCnt) / _imuInternalFrequency - 1.0 / _imuFrequency; | |
| 1528 | LOG_DATA("{}: Simulated internal IMU data for time [{}] (_imuUpdateCnt {}, _imuInternalUpdateCnt {})", nameId(), | ||
| 1529 | (_startTime + std::chrono::duration<double>(imuInternalUpdateTime)).toYMDHMS(), _imuUpdateCnt, _imuInternalUpdateCnt); | ||
| 1530 | |||
| 1531 | // --------------------------------------------------------- Calculation of data ----------------------------------------------------------- | ||
| 1532 |
1/2✓ Branch 1 taken 2868986 times.
✗ Branch 2 not taken.
|
2866616 | Eigen::Vector3d lla_position = lla_calcPosition(imuInternalUpdateTime); |
| 1533 | LOG_DATA("{}: [{:8.3f}] lla_position = {}°, {}°, {} m", nameId(), imuInternalUpdateTime, rad2deg(lla_position(0)), rad2deg(lla_position(1)), lla_position(2)); | ||
| 1534 |
1/2✓ Branch 1 taken 2865539 times.
✗ Branch 2 not taken.
|
2868986 | Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_position); |
| 1535 |
3/6✓ Branch 1 taken 2865361 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2867272 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2870512 times.
✗ Branch 8 not taken.
|
2865539 | Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1)); |
| 1536 | |||
| 1537 |
1/2✓ Branch 1 taken 2866620 times.
✗ Branch 2 not taken.
|
2870512 | Eigen::Vector3d n_vel = n_calcVelocity(imuInternalUpdateTime, n_Quat_e); |
| 1538 | LOG_DATA("{}: [{:8.3f}] n_vel = {} [m/s]", nameId(), imuInternalUpdateTime, n_vel.transpose()); | ||
| 1539 | |||
| 1540 |
1/2✓ Branch 1 taken 2871254 times.
✗ Branch 2 not taken.
|
2866620 | auto [roll, pitch, yaw] = calcFlightAngles(imuInternalUpdateTime); |
| 1541 | LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), imuInternalUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw)); | ||
| 1542 | |||
| 1543 |
1/2✓ Branch 1 taken 2866903 times.
✗ Branch 2 not taken.
|
2863081 | Eigen::Quaterniond b_Quat_n = trafo::b_Quat_n(roll, pitch, yaw); |
| 1544 | |||
| 1545 |
1/2✓ Branch 1 taken 2865053 times.
✗ Branch 2 not taken.
|
2866903 | Eigen::Vector3d n_omega_ie = n_Quat_e * InsConst::e_omega_ie; |
| 1546 | LOG_DATA("{}: [{:8.3f}] n_omega_ie = {} [rad/s]", nameId(), imuInternalUpdateTime, n_omega_ie.transpose()); | ||
| 1547 |
2/4✓ Branch 1 taken 2863179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2861872 times.
✗ Branch 5 not taken.
|
2865053 | auto R_N = calcEarthRadius_N(lla_position(0)); |
| 1548 | LOG_DATA("{}: [{:8.3f}] R_N = {} [m]", nameId(), imuInternalUpdateTime, R_N); | ||
| 1549 |
2/4✓ Branch 1 taken 2865264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2866965 times.
✗ Branch 5 not taken.
|
2861872 | auto R_E = calcEarthRadius_E(lla_position(0)); |
| 1550 | LOG_DATA("{}: [{:8.3f}] R_E = {} [m]", nameId(), imuInternalUpdateTime, R_E); | ||
| 1551 |
1/2✓ Branch 1 taken 2869325 times.
✗ Branch 2 not taken.
|
2866965 | Eigen::Vector3d n_omega_en = n_calcTransportRate(lla_position, n_vel, R_N, R_E); |
| 1552 | LOG_DATA("{}: [{:8.3f}] n_omega_en = {} [rad/s]", nameId(), imuInternalUpdateTime, n_omega_en.transpose()); | ||
| 1553 | |||
| 1554 | // ------------------------------------------------------------ Accelerations -------------------------------------------------------------- | ||
| 1555 | |||
| 1556 | // Force to keep vehicle on track | ||
| 1557 |
1/2✓ Branch 1 taken 2871922 times.
✗ Branch 2 not taken.
|
2869325 | Eigen::Vector3d n_trajectoryAccel = n_calcTrajectoryAccel(imuInternalUpdateTime, n_Quat_e, lla_position, n_vel); |
| 1558 | LOG_DATA("{}: [{:8.3f}] n_trajectoryAccel = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_trajectoryAccel.transpose()); | ||
| 1559 | |||
| 1560 | // Measured acceleration in local-navigation frame coordinates [m/s^2] | ||
| 1561 |
1/2✓ Branch 1 taken 2848123 times.
✗ Branch 2 not taken.
|
2871922 | Eigen::Vector3d n_accel = n_trajectoryAccel; |
| 1562 |
1/2✓ Branch 0 taken 2849612 times.
✗ Branch 1 not taken.
|
2848123 | if (_coriolisAccelerationEnabled) // Apply Coriolis Acceleration |
| 1563 | { | ||
| 1564 |
1/2✓ Branch 1 taken 2865281 times.
✗ Branch 2 not taken.
|
2849612 | Eigen::Vector3d n_coriolisAcceleration = n_calcCoriolisAcceleration(n_omega_ie, n_omega_en, n_vel); |
| 1565 | LOG_DATA("{}: [{:8.3f}] n_coriolisAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_coriolisAcceleration.transpose()); | ||
| 1566 |
1/2✓ Branch 1 taken 2859716 times.
✗ Branch 2 not taken.
|
2865281 | n_accel += n_coriolisAcceleration; |
| 1567 | } | ||
| 1568 | |||
| 1569 | // Mass attraction of the Earth (gravitation) | ||
| 1570 |
1/2✓ Branch 1 taken 2862856 times.
✗ Branch 2 not taken.
|
2858227 | Eigen::Vector3d n_gravitation = n_calcGravitation(lla_position, _gravitationModel); |
| 1571 | LOG_DATA("{}: [{:8.3f}] n_gravitation = {} [m/s^2] ({})", nameId(), imuInternalUpdateTime, n_gravitation.transpose(), NAV::to_string(_gravitationModel)); | ||
| 1572 |
1/2✓ Branch 1 taken 2857070 times.
✗ Branch 2 not taken.
|
2862856 | n_accel -= n_gravitation; // Apply the local gravity vector |
| 1573 | |||
| 1574 |
1/2✓ Branch 0 taken 2857740 times.
✗ Branch 1 not taken.
|
2857070 | if (_centrifgalAccelerationEnabled) // Centrifugal acceleration caused by the Earth's rotation |
| 1575 | { | ||
| 1576 |
1/2✓ Branch 1 taken 2867192 times.
✗ Branch 2 not taken.
|
2857740 | Eigen::Vector3d e_centrifugalAcceleration = e_calcCentrifugalAcceleration(e_position); |
| 1577 | LOG_DATA("{}: [{:8.3f}] e_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, e_centrifugalAcceleration.transpose()); | ||
| 1578 |
1/2✓ Branch 1 taken 2860718 times.
✗ Branch 2 not taken.
|
2867192 | Eigen::Vector3d n_centrifugalAcceleration = n_Quat_e * e_centrifugalAcceleration; |
| 1579 | LOG_DATA("{}: [{:8.3f}] n_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_centrifugalAcceleration.transpose()); | ||
| 1580 |
1/2✓ Branch 1 taken 2862763 times.
✗ Branch 2 not taken.
|
2860718 | n_accel += n_centrifugalAcceleration; |
| 1581 | } | ||
| 1582 | |||
| 1583 | // Acceleration measured by the accelerometer in platform coordinates | ||
| 1584 |
3/6✓ Branch 1 taken 2865906 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2868152 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2863526 times.
✗ Branch 8 not taken.
|
2862093 | Eigen::Vector3d p_accel = _imuPos.p_quatAccel_b() * b_Quat_n * n_accel; |
| 1585 | LOG_DATA("{}: [{:8.3f}] p_accel = {} [m/s^2]", nameId(), imuInternalUpdateTime, p_accel.transpose()); | ||
| 1586 | |||
| 1587 | // ------------------------------------------------------------ Angular rates -------------------------------------------------------------- | ||
| 1588 | |||
| 1589 |
3/6✓ Branch 1 taken 2866118 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2863934 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2867997 times.
✗ Branch 8 not taken.
|
2863526 | Eigen::Vector3d n_omega_nb = n_calcOmega_nb(imuInternalUpdateTime, Eigen::Vector3d{ roll, pitch, yaw }, b_Quat_n.conjugate()); |
| 1590 | |||
| 1591 | // ω_ib_n = ω_in_n + ω_nb_n = (ω_ie_n + ω_en_n) + ω_nb_n | ||
| 1592 |
1/2✓ Branch 1 taken 2860731 times.
✗ Branch 2 not taken.
|
2867997 | Eigen::Vector3d n_omega_ib = n_omega_nb; |
| 1593 |
1/2✓ Branch 0 taken 2862121 times.
✗ Branch 1 not taken.
|
2860731 | if (_angularRateEarthRotationEnabled) |
| 1594 | { | ||
| 1595 |
1/2✓ Branch 1 taken 2861155 times.
✗ Branch 2 not taken.
|
2862121 | n_omega_ib += n_omega_ie; |
| 1596 | } | ||
| 1597 |
1/2✓ Branch 0 taken 2862357 times.
✗ Branch 1 not taken.
|
2859765 | if (_angularRateTransportRateEnabled) |
| 1598 | { | ||
| 1599 |
1/2✓ Branch 1 taken 2865715 times.
✗ Branch 2 not taken.
|
2862357 | n_omega_ib += n_omega_en; |
| 1600 | } | ||
| 1601 | |||
| 1602 | // ω_ib_b = b_Quat_n * ω_ib_n | ||
| 1603 | // = 0 | ||
| 1604 | // ω_ip_p = p_Quat_b * (ω_ib_b + ω_bp_b) = p_Quat_b * ω_ib_b | ||
| 1605 |
3/6✓ Branch 1 taken 2865951 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2868123 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2864636 times.
✗ Branch 8 not taken.
|
2863123 | Eigen::Vector3d p_omega_ip = _imuPos.p_quatGyro_b() * b_Quat_n * n_omega_ib; |
| 1606 | LOG_DATA("{}: [{:8.3f}] p_omega_ip = {} [rad/s]", nameId(), imuInternalUpdateTime, p_omega_ip.transpose()); | ||
| 1607 | |||
| 1608 | // -------------------------------------------------- Construct the message to send out ---------------------------------------------------- | ||
| 1609 | |||
| 1610 |
2/4✓ Branch 1 taken 2855391 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2860090 times.
✗ Branch 6 not taken.
|
2864636 | obs->p_acceleration = p_accel.cast<double>(); |
| 1611 |
2/4✓ Branch 1 taken 2859745 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2864876 times.
✗ Branch 6 not taken.
|
2860090 | obs->p_angularRate = p_omega_ip.cast<double>(); |
| 1612 | // obs->p_magneticField.emplace(0, 0, 0); | ||
| 1613 | |||
| 1614 |
1/2✓ Branch 1 taken 2866463 times.
✗ Branch 2 not taken.
|
2864876 | Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate(); |
| 1615 | |||
| 1616 |
2/4✓ Branch 1 taken 2861956 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2863331 times.
✗ Branch 6 not taken.
|
2866463 | obs->n_accelDynamics = n_trajectoryAccel.cast<double>(); |
| 1617 |
2/4✓ Branch 1 taken 2863226 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2866280 times.
✗ Branch 6 not taken.
|
2863331 | obs->n_angularRateDynamics = n_omega_nb.cast<double>(); |
| 1618 | |||
| 1619 |
3/6✓ Branch 1 taken 2864367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2861680 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2864634 times.
✗ Branch 9 not taken.
|
2866280 | obs->e_accelDynamics = (e_Quat_n * n_trajectoryAccel).cast<double>(); |
| 1620 |
3/6✓ Branch 1 taken 2864666 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2861951 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2865144 times.
✗ Branch 9 not taken.
|
2864634 | obs->e_angularRateDynamics = (e_Quat_n * n_omega_nb).cast<double>(); |
| 1621 | |||
| 1622 | 2865144 | double dt = 1.0 / _imuInternalFrequency; | |
| 1623 |
1/2✓ Branch 0 taken 2865797 times.
✗ Branch 1 not taken.
|
2865144 | if (_imuInternalUpdateCnt != 0) |
| 1624 | { | ||
| 1625 | 2865797 | dTime += dt; | |
| 1626 | |||
| 1627 |
4/8✓ Branch 1 taken 2866835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2859643 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2861901 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2861509 times.
✗ Branch 11 not taken.
|
2865797 | p_deltaVel += (p_accel + _p_lastImuAccelerationMeas) / 2 * dt; |
| 1628 |
4/8✓ Branch 1 taken 2865500 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2861853 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2863816 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2865044 times.
✗ Branch 11 not taken.
|
2861509 | p_deltaTheta += (p_omega_ip + _p_lastImuAngularRateMeas) / 2 * dt; |
| 1629 | } | ||
| 1630 |
1/2✓ Branch 1 taken 2863876 times.
✗ Branch 2 not taken.
|
2864391 | _p_lastImuAccelerationMeas = p_accel; |
| 1631 |
1/2✓ Branch 1 taken 2866617 times.
✗ Branch 2 not taken.
|
2863876 | _p_lastImuAngularRateMeas = p_omega_ip; |
| 1632 | |||
| 1633 | 2866617 | _imuInternalUpdateCnt++; | |
| 1634 |
2/2✓ Branch 0 taken 2831407 times.
✓ Branch 1 taken 35210 times.
|
2866617 | } while (imuInternalUpdateTime + 1e-6 < imuUpdateTime); |
| 1635 | |||
| 1636 | 35210 | obs->dtime = dTime; | |
| 1637 |
2/4✓ Branch 1 taken 35207 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 35208 times.
✗ Branch 6 not taken.
|
35210 | obs->dvel = p_deltaVel.cast<double>(); |
| 1638 |
2/4✓ Branch 1 taken 35208 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 35208 times.
✗ Branch 6 not taken.
|
35208 | obs->dtheta = p_deltaTheta.cast<double>(); |
| 1639 | LOG_DATA("{}: dtime = {}, dvel = {}, dtheta = {}", nameId(), obs->dtime, obs->dvel.transpose(), obs->dtheta.transpose()); | ||
| 1640 | |||
| 1641 | 35208 | _imuUpdateCnt++; | |
| 1642 |
1/2✓ Branch 2 taken 35213 times.
✗ Branch 3 not taken.
|
35208 | invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs); |
| 1643 | |||
| 1644 | 35213 | return obs; | |
| 1645 | 35206 | } | |
| 1646 | |||
| 1647 | 50018 | std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollPosVelAtt(size_t /* pinIdx */, bool peek) | |
| 1648 | { | ||
| 1649 | 50018 | auto gnssUpdateTime = static_cast<double>(_gnssUpdateCnt) / _gnssFrequency; | |
| 1650 | |||
| 1651 |
1/2✓ Branch 1 taken 50018 times.
✗ Branch 2 not taken.
|
50018 | Eigen::Vector3d lla_position = lla_calcPosition(gnssUpdateTime); |
| 1652 | |||
| 1653 | // -------------------------------------------------- Check if a stop condition is met ----------------------------------------------------- | ||
| 1654 |
3/4✓ Branch 1 taken 50018 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 50016 times.
|
50018 | if (checkStopCondition(gnssUpdateTime, lla_position)) |
| 1655 | { | ||
| 1656 | 2 | return nullptr; | |
| 1657 | } | ||
| 1658 | |||
| 1659 | // ------------------------------------- Early return in case of peeking to avoid heavy calculations --------------------------------------- | ||
| 1660 |
2/2✓ Branch 0 taken 25008 times.
✓ Branch 1 taken 25008 times.
|
50016 | if (peek) |
| 1661 | { | ||
| 1662 |
1/2✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
|
25008 | auto obs = std::make_shared<NodeData>(); |
| 1663 |
2/4✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25008 times.
✗ Branch 6 not taken.
|
25008 | obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime); |
| 1664 | 25008 | return obs; | |
| 1665 | 25008 | } | |
| 1666 |
1/2✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
|
25008 | auto obs = std::make_shared<PosVelAtt>(); |
| 1667 |
2/4✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25008 times.
✗ Branch 6 not taken.
|
25008 | obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime); |
| 1668 | LOG_DATA("{}: Simulating GNSS data for time [{}]", nameId(), obs->insTime.toYMDHMS()); | ||
| 1669 | |||
| 1670 | // --------------------------------------------------------- Calculation of data ----------------------------------------------------------- | ||
| 1671 | LOG_DATA("{}: [{:8.3f}] lla_position = {}°, {}°, {} m", nameId(), gnssUpdateTime, rad2deg(lla_position(0)), rad2deg(lla_position(1)), lla_position(2)); | ||
| 1672 |
3/6✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
|
25008 | Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1)); |
| 1673 |
1/2✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
|
25008 | Eigen::Vector3d n_vel = n_calcVelocity(gnssUpdateTime, n_Quat_e); |
| 1674 | LOG_DATA("{}: [{:8.3f}] n_vel = {} [m/s]", nameId(), gnssUpdateTime, n_vel.transpose()); | ||
| 1675 |
1/2✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
|
25008 | auto [roll, pitch, yaw] = calcFlightAngles(gnssUpdateTime); |
| 1676 | LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), gnssUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw)); | ||
| 1677 | |||
| 1678 | // -------------------------------------------------- Construct the message to send out ---------------------------------------------------- | ||
| 1679 | |||
| 1680 |
5/10✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25008 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25008 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25008 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 25008 times.
✗ Branch 15 not taken.
|
25008 | obs->setState_n(lla_position.cast<double>(), n_vel.cast<double>(), trafo::n_Quat_b(roll, pitch, yaw).cast<double>()); |
| 1681 | |||
| 1682 | 25008 | _gnssUpdateCnt++; | |
| 1683 |
1/2✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
|
25008 | invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, obs); |
| 1684 | |||
| 1685 | 25008 | return obs; | |
| 1686 | 25008 | } | |
| 1687 | |||
| 1688 | 2891144 | std::array<double, 3> NAV::ImuSimulator::calcFlightAngles(double time) const | |
| 1689 | { | ||
| 1690 | return { | ||
| 1691 | 2891144 | static_cast<double>(_splines.roll(time)), | |
| 1692 | 5789142 | static_cast<double>(_splines.pitch(time)), | |
| 1693 | 5791967 | static_cast<double>(_splines.yaw(time)) | |
| 1694 | 2893807 | }; | |
| 1695 | } | ||
| 1696 | |||
| 1697 | 2990780 | Eigen::Vector3d NAV::ImuSimulator::lla_calcPosition(double time) const | |
| 1698 | { | ||
| 1699 | ✗ | Eigen::Vector3d e_pos(static_cast<double>(_splines.x(time)), | |
| 1700 |
1/2✓ Branch 1 taken 2991423 times.
✗ Branch 2 not taken.
|
2990034 | static_cast<double>(_splines.y(time)), |
| 1701 |
3/6✓ Branch 1 taken 2985952 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2990034 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2986077 times.
✗ Branch 8 not taken.
|
5982203 | static_cast<double>(_splines.z(time))); |
| 1702 |
1/2✓ Branch 1 taken 2984825 times.
✗ Branch 2 not taken.
|
5970902 | return trafo::ecef2lla_WGS84(e_pos); |
| 1703 | } | ||
| 1704 | |||
| 1705 | 2895163 | Eigen::Vector3d NAV::ImuSimulator::n_calcVelocity(double time, const Eigen::Quaterniond& n_Quat_e) const | |
| 1706 | { | ||
| 1707 | ✗ | Eigen::Vector3d e_vel(static_cast<double>(_splines.x.derivative(1, time)), | |
| 1708 |
1/2✓ Branch 1 taken 2896242 times.
✗ Branch 2 not taken.
|
2894527 | static_cast<double>(_splines.y.derivative(1, time)), |
| 1709 |
3/6✓ Branch 1 taken 2887267 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2894527 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2887684 times.
✗ Branch 8 not taken.
|
5791405 | static_cast<double>(_splines.z.derivative(1, time))); |
| 1710 |
1/2✓ Branch 1 taken 2888555 times.
✗ Branch 2 not taken.
|
5776239 | return n_Quat_e * e_vel; |
| 1711 | } | ||
| 1712 | |||
| 1713 | 2869707 | Eigen::Vector3d NAV::ImuSimulator::n_calcTrajectoryAccel(double time, | |
| 1714 | const Eigen::Quaterniond& n_Quat_e, | ||
| 1715 | const Eigen::Vector3d& lla_position, | ||
| 1716 | const Eigen::Vector3d& n_velocity) const | ||
| 1717 | { | ||
| 1718 | ✗ | Eigen::Vector3d e_accel(static_cast<double>(_splines.x.derivative(2, time)), | |
| 1719 |
1/2✓ Branch 1 taken 2870943 times.
✗ Branch 2 not taken.
|
2870157 | static_cast<double>(_splines.y.derivative(2, time)), |
| 1720 |
3/6✓ Branch 1 taken 2863737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2870157 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2865740 times.
✗ Branch 8 not taken.
|
5740650 | static_cast<double>(_splines.z.derivative(2, time))); |
| 1721 |
1/2✓ Branch 1 taken 2866054 times.
✗ Branch 2 not taken.
|
2865740 | Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate(); |
| 1722 |
1/2✓ Branch 1 taken 2864751 times.
✗ Branch 2 not taken.
|
2866054 | Eigen::Vector3d e_vel = e_Quat_n * n_velocity; |
| 1723 | |||
| 1724 | // Math: \dot{C}_n^e = C_n^e \cdot \Omega_{en}^n | ||
| 1725 |
1/2✓ Branch 1 taken 2868065 times.
✗ Branch 2 not taken.
|
2869270 | Eigen::Matrix3d n_DCM_dot_e = e_Quat_n.toRotationMatrix() |
| 1726 |
1/2✓ Branch 1 taken 2869270 times.
✗ Branch 2 not taken.
|
2869120 | * math::skewSymmetricMatrix(n_calcTransportRate(lla_position, n_velocity, |
| 1727 |
2/4✓ Branch 1 taken 2868542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2869120 times.
✗ Branch 5 not taken.
|
2863894 | calcEarthRadius_N(lla_position(0)), |
| 1728 |
5/10✓ Branch 1 taken 2860549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2865430 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2863894 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2859232 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2865412 times.
✗ Branch 14 not taken.
|
5732816 | calcEarthRadius_E(lla_position(0)))); |
| 1729 | |||
| 1730 | // Math: \dot{C}_e^n = (\dot{C}_n^e)^T | ||
| 1731 |
2/4✓ Branch 1 taken 2848866 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2861477 times.
✗ Branch 5 not taken.
|
2865412 | Eigen::Matrix3d e_DCM_dot_n = n_DCM_dot_e.transpose(); |
| 1732 | |||
| 1733 | // 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 | ||
| 1734 |
4/8✓ Branch 1 taken 2863138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2850749 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2858617 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2861722 times.
✗ Branch 11 not taken.
|
2861477 | return e_DCM_dot_n * e_vel + n_Quat_e * e_accel; |
| 1735 | } | ||
| 1736 | |||
| 1737 | 2861154 | Eigen::Vector3d NAV::ImuSimulator::n_calcOmega_nb(double time, const Eigen::Vector3d& rollPitchYaw, const Eigen::Quaterniond& n_Quat_b) const | |
| 1738 | { | ||
| 1739 |
1/2✓ Branch 1 taken 2859774 times.
✗ Branch 2 not taken.
|
2861154 | const auto& R = rollPitchYaw(0); |
| 1740 |
1/2✓ Branch 1 taken 2863908 times.
✗ Branch 2 not taken.
|
2859774 | const auto& P = rollPitchYaw(1); |
| 1741 | |||
| 1742 | // ######################################################################################################################################### | ||
| 1743 | |||
| 1744 |
1/2✓ Branch 1 taken 2863175 times.
✗ Branch 2 not taken.
|
2863908 | auto R_dot = static_cast<double>(_splines.roll.derivative(1, time)); |
| 1745 |
1/2✓ Branch 1 taken 2868845 times.
✗ Branch 2 not taken.
|
2863175 | auto Y_dot = static_cast<double>(_splines.yaw.derivative(1, time)); |
| 1746 |
1/2✓ Branch 1 taken 2870398 times.
✗ Branch 2 not taken.
|
2868845 | auto P_dot = static_cast<double>(_splines.pitch.derivative(1, time)); |
| 1747 | |||
| 1748 | 5723209 | auto C_3 = [](auto R) { | |
| 1749 | // Eigen::Matrix3d C; | ||
| 1750 | // C << 1, 0 , 0 , | ||
| 1751 | // 0, std::cos(R), std::sin(R), | ||
| 1752 | // 0, -std::sin(R), std::cos(R); | ||
| 1753 | // return C; | ||
| 1754 |
2/4✓ Branch 1 taken 5714728 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5729761 times.
✗ Branch 5 not taken.
|
5723209 | return Eigen::AngleAxisd{ -R, Eigen::Vector3d::UnitX() }; |
| 1755 | }; | ||
| 1756 | 2865365 | auto C_2 = [](auto P) { | |
| 1757 | // Eigen::Matrix3d C; | ||
| 1758 | // C << std::cos(P), 0 , -std::sin(P), | ||
| 1759 | // 0 , 1 , 0 , | ||
| 1760 | // std::sin(P), 0 , std::cos(P); | ||
| 1761 | // return C; | ||
| 1762 |
2/4✓ Branch 1 taken 2855117 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2861618 times.
✗ Branch 5 not taken.
|
2865365 | return Eigen::AngleAxisd{ -P, Eigen::Vector3d::UnitY() }; |
| 1763 | }; | ||
| 1764 | |||
| 1765 | // ω_nb_b = [∂/∂t R] + C_3 [ 0 ] + C_3 C_2 [ 0 ] | ||
| 1766 | // [ 0 ] [∂/∂t P] [ 0 ] | ||
| 1767 | // [ 0 ] [ 0 ] [∂/∂t Y] | ||
| 1768 |
1/2✓ Branch 1 taken 2859407 times.
✗ Branch 2 not taken.
|
2863848 | Eigen::Vector3d b_omega_nb = Eigen::Vector3d{ R_dot, 0, 0 } |
| 1769 |
4/8✓ Branch 1 taken 2864910 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2865419 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2863848 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2857114 times.
✗ Branch 11 not taken.
|
5723162 | + C_3(R) * Eigen::Vector3d{ 0, P_dot, 0 } |
| 1770 |
7/14✓ Branch 1 taken 2866403 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2864301 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2866286 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2869320 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2863755 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2857970 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2858673 times.
✗ Branch 20 not taken.
|
5727512 | + C_3(R) * C_2(P) * Eigen::Vector3d{ 0, 0, Y_dot }; |
| 1771 | |||
| 1772 |
1/2✓ Branch 1 taken 2862581 times.
✗ Branch 2 not taken.
|
5721254 | return n_Quat_b * b_omega_nb; |
| 1773 | } | ||
| 1774 | |||
| 1775 | ✗ | const char* NAV::ImuSimulator::to_string(TrajectoryType value) | |
| 1776 | { | ||
| 1777 | ✗ | switch (value) | |
| 1778 | { | ||
| 1779 | ✗ | case TrajectoryType::Fixed: | |
| 1780 | ✗ | return "Fixed"; | |
| 1781 | ✗ | case TrajectoryType::Linear: | |
| 1782 | ✗ | return "Linear"; | |
| 1783 | ✗ | case TrajectoryType::Circular: | |
| 1784 | ✗ | return "Circular"; | |
| 1785 | ✗ | case TrajectoryType::Csv: | |
| 1786 | ✗ | return "CSV"; | |
| 1787 | ✗ | case TrajectoryType::RoseFigure: | |
| 1788 | ✗ | return "Rose Figure"; | |
| 1789 | ✗ | case TrajectoryType::COUNT: | |
| 1790 | ✗ | return ""; | |
| 1791 | } | ||
| 1792 | ✗ | return ""; | |
| 1793 | } | ||
| 1794 | |||
| 1795 | ✗ | const char* NAV::ImuSimulator::to_string(Direction value) | |
| 1796 | { | ||
| 1797 | ✗ | switch (value) | |
| 1798 | { | ||
| 1799 | ✗ | case Direction::CW: | |
| 1800 | ✗ | return "Clockwise (CW)"; | |
| 1801 | ✗ | case Direction::CCW: | |
| 1802 | ✗ | return "Counterclockwise (CCW)"; | |
| 1803 | ✗ | case Direction::COUNT: | |
| 1804 | ✗ | return ""; | |
| 1805 | } | ||
| 1806 | ✗ | return ""; | |
| 1807 | } | ||
| 1808 |