| 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 "PosVelAttInitializer.hpp" | ||
| 10 | #include "NodeRegistry.hpp" | ||
| 11 | |||
| 12 | #include "util/Logger.hpp" | ||
| 13 | |||
| 14 | #include "internal/NodeManager.hpp" | ||
| 15 | namespace nm = NAV::NodeManager; | ||
| 16 | #include "internal/FlowManager.hpp" | ||
| 17 | |||
| 18 | #include "internal/gui/widgets/EnumCombo.hpp" | ||
| 19 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 20 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 21 | #include "internal/gui/NodeEditorApplication.hpp" | ||
| 22 | |||
| 23 | #include "util/Time/TimeBase.hpp" | ||
| 24 | #include "util/Vendor/Ublox/UbloxTypes.hpp" | ||
| 25 | #include "Navigation/INS/Functions.hpp" | ||
| 26 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 27 | #include "Navigation/Transformations/Units.hpp" | ||
| 28 | |||
| 29 | #include "NodeData/State/PosVelAtt.hpp" | ||
| 30 | #include "NodeData/GNSS/SppSolution.hpp" | ||
| 31 | |||
| 32 | #include <limits> | ||
| 33 | |||
| 34 | 115 | NAV::PosVelAttInitializer::PosVelAttInitializer() | |
| 35 |
9/18✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 115 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 115 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 115 times.
✗ Branch 16 not taken.
✓ Branch 21 taken 115 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 115 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 115 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 115 times.
✗ Branch 31 not taken.
|
115 | : Node(typeStatic()) |
| 36 | { | ||
| 37 | LOG_TRACE("{}: called", name); | ||
| 38 | |||
| 39 | 115 | _hasConfig = true; | |
| 40 | 115 | _guiConfigDefaultWindowSize = { 345, 342 }; | |
| 41 | |||
| 42 |
4/8✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 115 times.
✓ Branch 10 taken 115 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
460 | nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() }); |
| 43 | |||
| 44 |
1/2✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
|
115 | updatePins(); |
| 45 | 230 | } | |
| 46 | |||
| 47 | 232 | NAV::PosVelAttInitializer::~PosVelAttInitializer() | |
| 48 | { | ||
| 49 | LOG_TRACE("{}: called", nameId()); | ||
| 50 | 232 | } | |
| 51 | |||
| 52 | 229 | std::string NAV::PosVelAttInitializer::typeStatic() | |
| 53 | { | ||
| 54 |
1/2✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
|
458 | return "PosVelAttInitializer"; |
| 55 | } | ||
| 56 | |||
| 57 | ✗ | std::string NAV::PosVelAttInitializer::type() const | |
| 58 | { | ||
| 59 | ✗ | return typeStatic(); | |
| 60 | } | ||
| 61 | |||
| 62 | 114 | std::string NAV::PosVelAttInitializer::category() | |
| 63 | { | ||
| 64 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
228 | return "State"; |
| 65 | } | ||
| 66 | |||
| 67 | ✗ | void NAV::PosVelAttInitializer::guiConfig() | |
| 68 | { | ||
| 69 | ✗ | if (_inputPinIdxIMU >= 0 && inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked() | |
| 70 | ✗ | && !(_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))) | |
| 71 | { | ||
| 72 | ✗ | ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio()); | |
| 73 | ✗ | if (ImGui::InputDouble(fmt::format("Initialization Duration Attitude##{}", size_t(id)).c_str(), &_initDuration, 0.0, 0.0, "%.3f s")) | |
| 74 | { | ||
| 75 | ✗ | flow::ApplyChanges(); | |
| 76 | } | ||
| 77 | } | ||
| 78 | |||
| 79 | ✗ | if (_inputPinIdxIMU >= 0 && inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked() | |
| 80 | ✗ | && _inputPinIdxGNSS >= 0 && inputPins.at(static_cast<size_t>(_inputPinIdxGNSS)).isPinLinked() | |
| 81 | ✗ | && !(_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2))) | |
| 82 | { | ||
| 83 | ✗ | ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio()); | |
| 84 | ✗ | if (gui::widgets::EnumCombo(fmt::format("Attitude Init Source##{}", size_t(id)).c_str(), _attitudeMode)) | |
| 85 | { | ||
| 86 | ✗ | LOG_DEBUG("{}: Attitude Init Source changed to {}", nameId(), to_string(_attitudeMode)); | |
| 87 | ✗ | flow::ApplyChanges(); | |
| 88 | } | ||
| 89 | } | ||
| 90 | |||
| 91 | ✗ | if (ImGui::BeginTable(fmt::format("Initialized State##{}", size_t(id)).c_str(), | |
| 92 | ✗ | 4, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) | |
| 93 | { | ||
| 94 | ✗ | ImGui::TableSetupColumn(""); | |
| 95 | ✗ | ImGui::TableSetupColumn(""); | |
| 96 | ✗ | ImGui::TableSetupColumn("Threshold"); | |
| 97 | ✗ | ImGui::TableSetupColumn("Accuracy"); | |
| 98 | ✗ | ImGui::TableHeadersRow(); | |
| 99 | |||
| 100 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 101 | /* Position */ | ||
| 102 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 103 | |||
| 104 | ✗ | ImGui::TableNextColumn(); | |
| 105 | ✗ | ImGui::Text("Position"); | |
| 106 | ✗ | ImGui::TableNextColumn(); | |
| 107 | ✗ | float size = 7.0F; | |
| 108 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F, | |
| 109 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 110 | size, | ||
| 111 | ✗ | _posVelAttInitialized.at(0) || _overridePosition | |
| 112 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 113 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 114 | ✗ | ImGui::Selectable(("##determinePosition" + std::to_string(size_t(id))).c_str(), | |
| 115 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(1.6F * size, 0.0F)); | |
| 116 | ✗ | if (ImGui::IsItemHovered()) | |
| 117 | { | ||
| 118 | ✗ | ImGui::SetTooltip("%s", _posVelAttInitialized.at(0) || _overridePosition ? "Successfully Initialized" : "To be initialized"); | |
| 119 | } | ||
| 120 | ✗ | ImGui::TableNextColumn(); | |
| 121 | ✗ | ImGui::SetNextItemWidth(-FLT_MIN); | |
| 122 | ✗ | if (ImGui::DragDouble(("##positionAccuracyThreshold" + std::to_string(size_t(id))).c_str(), | |
| 123 | &_positionAccuracyThreshold, 0.1F, 0.0, 1000.0, "%.1f cm")) | ||
| 124 | { | ||
| 125 | ✗ | flow::ApplyChanges(); | |
| 126 | } | ||
| 127 | ✗ | ImGui::TableNextColumn(); | |
| 128 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F, | |
| 129 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 130 | size, | ||
| 131 | ✗ | _lastPositionAccuracy.at(0) <= _positionAccuracyThreshold | |
| 132 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 133 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 134 | ✗ | ImGui::Selectable(("##lastPositionAccuracy.at(0)" + std::to_string(size_t(id))).c_str(), | |
| 135 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F)); | |
| 136 | ✗ | if (ImGui::IsItemHovered()) | |
| 137 | { | ||
| 138 | ✗ | ImGui::SetTooltip("Last: %.3f cm", _lastPositionAccuracy.at(0)); | |
| 139 | } | ||
| 140 | ✗ | ImGui::SameLine(); | |
| 141 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F, | |
| 142 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 143 | size, | ||
| 144 | ✗ | _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold | |
| 145 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 146 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 147 | ✗ | ImGui::Selectable(("##lastPositionAccuracy.at(1)" + std::to_string(size_t(id))).c_str(), | |
| 148 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F)); | |
| 149 | ✗ | if (ImGui::IsItemHovered()) | |
| 150 | { | ||
| 151 | ✗ | ImGui::SetTooltip("Last: %.3f cm", _lastPositionAccuracy.at(1)); | |
| 152 | } | ||
| 153 | ✗ | ImGui::SameLine(); | |
| 154 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F, | |
| 155 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 156 | size, | ||
| 157 | ✗ | _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold | |
| 158 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 159 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 160 | ✗ | ImGui::Selectable(("##lastPositionAccuracy.at(2)" + std::to_string(size_t(id))).c_str(), | |
| 161 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F)); | |
| 162 | ✗ | if (ImGui::IsItemHovered()) | |
| 163 | { | ||
| 164 | ✗ | ImGui::SetTooltip("Last: %.3f cm", _lastPositionAccuracy.at(2)); | |
| 165 | } | ||
| 166 | |||
| 167 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 168 | /* Velocity */ | ||
| 169 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 170 | |||
| 171 | ✗ | ImGui::TableNextColumn(); | |
| 172 | ✗ | ImGui::Text("Velocity"); | |
| 173 | ✗ | ImGui::TableNextColumn(); | |
| 174 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F, | |
| 175 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 176 | size, | ||
| 177 | ✗ | _posVelAttInitialized.at(1) || _overrideVelocity != VelocityOverride::OFF | |
| 178 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 179 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 180 | ✗ | ImGui::Selectable(("##determineVelocity" + std::to_string(size_t(id))).c_str(), | |
| 181 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(1.6F * size, 0.0F)); | |
| 182 | ✗ | if (ImGui::IsItemHovered()) | |
| 183 | { | ||
| 184 | ✗ | ImGui::SetTooltip("%s", _posVelAttInitialized.at(1) || _overrideVelocity != VelocityOverride::OFF | |
| 185 | ? "Successfully Initialized" | ||
| 186 | : "To be initialized"); | ||
| 187 | } | ||
| 188 | ✗ | ImGui::TableNextColumn(); | |
| 189 | ✗ | ImGui::SetNextItemWidth(-FLT_MIN); | |
| 190 | ✗ | if (ImGui::DragDouble(("##velocityAccuracyThreshold" + std::to_string(size_t(id))).c_str(), | |
| 191 | &_velocityAccuracyThreshold, 1.0F, 0.0, 1000.0, "%.0f cm/s")) | ||
| 192 | { | ||
| 193 | ✗ | flow::ApplyChanges(); | |
| 194 | } | ||
| 195 | ✗ | ImGui::TableNextColumn(); | |
| 196 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F, | |
| 197 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 198 | size, | ||
| 199 | ✗ | _lastVelocityAccuracy.at(0) <= _velocityAccuracyThreshold | |
| 200 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 201 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 202 | ✗ | ImGui::Selectable(("##lastVelocityAccuracy.at(0)" + std::to_string(size_t(id))).c_str(), | |
| 203 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F)); | |
| 204 | ✗ | if (ImGui::IsItemHovered()) | |
| 205 | { | ||
| 206 | ✗ | ImGui::SetTooltip("Last: %.3f cm", _lastVelocityAccuracy.at(0)); | |
| 207 | } | ||
| 208 | ✗ | ImGui::SameLine(); | |
| 209 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F, | |
| 210 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 211 | size, | ||
| 212 | ✗ | _lastVelocityAccuracy.at(1) <= _velocityAccuracyThreshold | |
| 213 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 214 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 215 | ✗ | ImGui::Selectable(("##lastVelocityAccuracy.at(1)" + std::to_string(size_t(id))).c_str(), | |
| 216 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F)); | |
| 217 | ✗ | if (ImGui::IsItemHovered()) | |
| 218 | { | ||
| 219 | ✗ | ImGui::SetTooltip("Last: %.3f cm", _lastVelocityAccuracy.at(1)); | |
| 220 | } | ||
| 221 | ✗ | ImGui::SameLine(); | |
| 222 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size * 1.2F, | |
| 223 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.8F), | |
| 224 | size, | ||
| 225 | ✗ | _lastVelocityAccuracy.at(2) <= _velocityAccuracyThreshold | |
| 226 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 227 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 228 | ✗ | ImGui::Selectable(("##lastVelocityAccuracy.at(2)" + std::to_string(size_t(id))).c_str(), | |
| 229 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(2.0F * size, 0.0F)); | |
| 230 | ✗ | if (ImGui::IsItemHovered()) | |
| 231 | { | ||
| 232 | ✗ | ImGui::SetTooltip("Last: %.3f cm", _lastVelocityAccuracy.at(2)); | |
| 233 | } | ||
| 234 | |||
| 235 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 236 | /* Attitude */ | ||
| 237 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 238 | |||
| 239 | ✗ | ImGui::TableNextColumn(); | |
| 240 | ✗ | ImGui::Text("Attitude"); | |
| 241 | ✗ | ImGui::TableNextColumn(); | |
| 242 | ✗ | ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F, | |
| 243 | ✗ | ImGui::GetCursorScreenPos().y + size * 1.4F), | |
| 244 | size, | ||
| 245 | ✗ | _posVelAttInitialized.at(2) || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)) | |
| 246 | ✗ | ? ImColor(0.0F, 255.0F, 0.0F) | |
| 247 | : ImColor(255.0F, 0.0F, 0.0F)); | ||
| 248 | ✗ | ImGui::Selectable(("##determineAttitude" + std::to_string(size_t(id))).c_str(), | |
| 249 | ✗ | false, ImGuiSelectableFlags_Disabled, ImVec2(1.6F * size, 0.0F)); | |
| 250 | ✗ | if (ImGui::IsItemHovered()) | |
| 251 | { | ||
| 252 | ✗ | ImGui::SetTooltip("%s", _posVelAttInitialized.at(2) || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)) | |
| 253 | ? "Successfully Initialized" | ||
| 254 | : "To be initialized"); | ||
| 255 | } | ||
| 256 | ✗ | ImGui::TableNextColumn(); | |
| 257 | |||
| 258 | ✗ | ImGui::EndTable(); | |
| 259 | } | ||
| 260 | |||
| 261 | ✗ | if (ImGui::Checkbox(fmt::format("Override Position##{}", size_t(id)).c_str(), &_overridePosition)) | |
| 262 | { | ||
| 263 | ✗ | updatePins(); | |
| 264 | ✗ | flow::ApplyChanges(); | |
| 265 | } | ||
| 266 | |||
| 267 | ✗ | if (_overridePosition) | |
| 268 | { | ||
| 269 | ✗ | ImGui::Indent(); | |
| 270 | ✗ | if (gui::widgets::PositionInput(fmt::format("Reference Frame##{}", size_t(id)).c_str(), _overridePositionValue, | |
| 271 | ✗ | gui::widgets::PositionInputLayout::SINGLE_COLUMN, 140 * gui::NodeEditorApplication::windowFontRatio())) | |
| 272 | { | ||
| 273 | ✗ | flow::ApplyChanges(); | |
| 274 | } | ||
| 275 | ✗ | ImGui::Unindent(); | |
| 276 | } | ||
| 277 | |||
| 278 | ✗ | ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio()); | |
| 279 | ✗ | if (gui::widgets::EnumCombo(fmt::format("Override Velocity##{}", size_t(id)).c_str(), _overrideVelocity)) | |
| 280 | { | ||
| 281 | ✗ | updatePins(); | |
| 282 | ✗ | flow::ApplyChanges(); | |
| 283 | } | ||
| 284 | ✗ | if (_overrideVelocity != VelocityOverride::OFF) | |
| 285 | { | ||
| 286 | ✗ | ImGui::Indent(); | |
| 287 | |||
| 288 | ✗ | bool ned = _overrideVelocity == VelocityOverride::NED; | |
| 289 | ✗ | ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio()); | |
| 290 | ✗ | if (ImGui::DragDouble(fmt::format("Velocity {} [m/s]##{}", ned ? "North" : "ECEF X", size_t(id)).c_str(), &_overrideVelocityValues(0), | |
| 291 | 1.0F, 0.0, 0.0, "%.4f")) | ||
| 292 | { | ||
| 293 | ✗ | flow::ApplyChanges(); | |
| 294 | } | ||
| 295 | ✗ | ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio()); | |
| 296 | ✗ | if (ImGui::DragDouble(fmt::format("Velocity {} [m/s]##{}", ned ? "Eeast" : "ECEF Y", size_t(id)).c_str(), &_overrideVelocityValues(1), | |
| 297 | 1.0F, 0.0, 0.0, "%.4f")) | ||
| 298 | { | ||
| 299 | ✗ | flow::ApplyChanges(); | |
| 300 | } | ||
| 301 | ✗ | ImGui::SetNextItemWidth(100 * gui::NodeEditorApplication::windowFontRatio()); | |
| 302 | ✗ | if (ImGui::DragDouble(fmt::format("Velocity {} [m/s]##{}", ned ? "Down" : "ECEF Z", size_t(id)).c_str(), &_overrideVelocityValues(2), | |
| 303 | 1.0F, 0.0, 0.0, "%.4f")) | ||
| 304 | { | ||
| 305 | ✗ | flow::ApplyChanges(); | |
| 306 | } | ||
| 307 | |||
| 308 | ✗ | ImGui::Unindent(); | |
| 309 | } | ||
| 310 | |||
| 311 | ✗ | if (ImGui::BeginTable(("Overrides##" + std::to_string(size_t(id))).c_str(), | |
| 312 | ✗ | 2, ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) | |
| 313 | { | ||
| 314 | ✗ | ImGui::TableNextColumn(); | |
| 315 | ✗ | if (ImGui::Checkbox(("Override Roll##" + std::to_string(size_t(id))).c_str(), &_overrideRollPitchYaw.at(0))) | |
| 316 | { | ||
| 317 | ✗ | updatePins(); | |
| 318 | ✗ | flow::ApplyChanges(); | |
| 319 | } | ||
| 320 | ✗ | ImGui::TableNextColumn(); | |
| 321 | ✗ | if (_overrideRollPitchYaw.at(0)) | |
| 322 | { | ||
| 323 | ✗ | ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio()); | |
| 324 | ✗ | if (ImGui::DragDouble(("##overrideValuesRollPitchYaw.at(0)" + std::to_string(size_t(id))).c_str(), | |
| 325 | ✗ | &_overrideRollPitchYawValues.at(0), 1.0F, -180.0, 180.0, "%.3f °")) | |
| 326 | { | ||
| 327 | ✗ | flow::ApplyChanges(); | |
| 328 | } | ||
| 329 | } | ||
| 330 | |||
| 331 | ✗ | ImGui::TableNextColumn(); | |
| 332 | ✗ | if (ImGui::Checkbox(("Override Pitch##" + std::to_string(size_t(id))).c_str(), &_overrideRollPitchYaw.at(1))) | |
| 333 | { | ||
| 334 | ✗ | updatePins(); | |
| 335 | ✗ | flow::ApplyChanges(); | |
| 336 | } | ||
| 337 | ✗ | ImGui::TableNextColumn(); | |
| 338 | ✗ | if (_overrideRollPitchYaw.at(1)) | |
| 339 | { | ||
| 340 | ✗ | ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio()); | |
| 341 | ✗ | if (ImGui::DragDouble(("##overrideValuesRollPitchYaw.at(1)" + std::to_string(size_t(id))).c_str(), | |
| 342 | ✗ | &_overrideRollPitchYawValues.at(1), 1.0F, -90.0, 90.0, "%.3f °")) | |
| 343 | { | ||
| 344 | ✗ | flow::ApplyChanges(); | |
| 345 | } | ||
| 346 | } | ||
| 347 | |||
| 348 | ✗ | ImGui::TableNextColumn(); | |
| 349 | ✗ | if (ImGui::Checkbox(("Override Yaw##" + std::to_string(size_t(id))).c_str(), &_overrideRollPitchYaw.at(2))) | |
| 350 | { | ||
| 351 | ✗ | updatePins(); | |
| 352 | ✗ | flow::ApplyChanges(); | |
| 353 | } | ||
| 354 | ✗ | ImGui::TableNextColumn(); | |
| 355 | ✗ | if (_overrideRollPitchYaw.at(2)) | |
| 356 | { | ||
| 357 | ✗ | ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio()); | |
| 358 | ✗ | if (ImGui::DragDouble(("##overrideValuesRollPitchYaw.at(2)" + std::to_string(size_t(id))).c_str(), | |
| 359 | ✗ | &_overrideRollPitchYawValues.at(2), 1.0, -180.0, 180.0, "%.3f °")) | |
| 360 | { | ||
| 361 | ✗ | flow::ApplyChanges(); | |
| 362 | } | ||
| 363 | } | ||
| 364 | |||
| 365 | ✗ | ImGui::EndTable(); | |
| 366 | } | ||
| 367 | |||
| 368 | ✗ | if (_overridePosition && _overrideVelocity != VelocityOverride::OFF | |
| 369 | ✗ | && _overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2]) | |
| 370 | { | ||
| 371 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 372 | ✗ | if (ImGui::TreeNode(fmt::format("Init Time##{}", size_t(id)).c_str())) | |
| 373 | { | ||
| 374 | ✗ | if (gui::widgets::TimeEdit(fmt::format("Init Time Edit {}", size_t(id)).c_str(), _initTime, _initTimeEditFormat)) | |
| 375 | { | ||
| 376 | ✗ | LOG_DEBUG("{}: initTime changed to {}", nameId(), _initTime); | |
| 377 | ✗ | flow::ApplyChanges(); | |
| 378 | } | ||
| 379 | ✗ | if (ImGui::Button("Reset")) | |
| 380 | { | ||
| 381 | ✗ | _initTime = InsTime(InsTime_GPSweekTow(0, 0, 0)); | |
| 382 | ✗ | flow::ApplyChanges(); | |
| 383 | } | ||
| 384 | ✗ | ImGui::TreePop(); | |
| 385 | } | ||
| 386 | } | ||
| 387 | ✗ | } | |
| 388 | |||
| 389 | ✗ | [[nodiscard]] json NAV::PosVelAttInitializer::save() const | |
| 390 | { | ||
| 391 | LOG_TRACE("{}: called", nameId()); | ||
| 392 | |||
| 393 | ✗ | json j; | |
| 394 | |||
| 395 | ✗ | j["initDuration"] = _initDuration; | |
| 396 | ✗ | j["attitudeMode"] = _attitudeMode; | |
| 397 | ✗ | j["positionAccuracyThreshold"] = _positionAccuracyThreshold; | |
| 398 | ✗ | j["velocityAccuracyThreshold"] = _velocityAccuracyThreshold; | |
| 399 | ✗ | j["overridePosition"] = _overridePosition; | |
| 400 | ✗ | j["overridePositionValues"] = _overridePositionValue; | |
| 401 | ✗ | j["overrideVelocity"] = _overrideVelocity; | |
| 402 | ✗ | j["overrideVelocityValues"] = _overrideVelocityValues; | |
| 403 | ✗ | j["overrideRollPitchYaw"] = _overrideRollPitchYaw; | |
| 404 | ✗ | j["overrideRollPitchYawValues"] = _overrideRollPitchYawValues; | |
| 405 | ✗ | j["initTime"] = _initTime; | |
| 406 | ✗ | j["initTimeEditFormat"] = _initTimeEditFormat; | |
| 407 | |||
| 408 | ✗ | return j; | |
| 409 | ✗ | } | |
| 410 | |||
| 411 | 1 | void NAV::PosVelAttInitializer::restore(json const& j) | |
| 412 | { | ||
| 413 | LOG_TRACE("{}: called", nameId()); | ||
| 414 | |||
| 415 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("initDuration")) |
| 416 | { | ||
| 417 | 1 | j.at("initDuration").get_to(_initDuration); | |
| 418 | } | ||
| 419 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("attitudeMode")) |
| 420 | { | ||
| 421 | 1 | j.at("attitudeMode").get_to(_attitudeMode); | |
| 422 | } | ||
| 423 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("positionAccuracyThreshold")) |
| 424 | { | ||
| 425 | 1 | j.at("positionAccuracyThreshold").get_to(_positionAccuracyThreshold); | |
| 426 | } | ||
| 427 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("velocityAccuracyThreshold")) |
| 428 | { | ||
| 429 | 1 | j.at("velocityAccuracyThreshold").get_to(_velocityAccuracyThreshold); | |
| 430 | } | ||
| 431 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("overridePosition")) |
| 432 | { | ||
| 433 | 1 | j.at("overridePosition").get_to(_overridePosition); | |
| 434 | } | ||
| 435 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("overridePositionValues")) |
| 436 | { | ||
| 437 | 1 | j.at("overridePositionValues").get_to(_overridePositionValue); | |
| 438 | } | ||
| 439 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("overrideVelocity")) |
| 440 | { | ||
| 441 | 1 | j.at("overrideVelocity").get_to(_overrideVelocity); | |
| 442 | } | ||
| 443 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("overrideVelocityValues")) |
| 444 | { | ||
| 445 | 1 | j.at("overrideVelocityValues").get_to(_overrideVelocityValues); | |
| 446 | } | ||
| 447 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("overrideRollPitchYaw")) |
| 448 | { | ||
| 449 | 1 | j.at("overrideRollPitchYaw").get_to(_overrideRollPitchYaw); | |
| 450 | } | ||
| 451 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("overrideRollPitchYawValues")) |
| 452 | { | ||
| 453 | 1 | j.at("overrideRollPitchYawValues").get_to(_overrideRollPitchYawValues); | |
| 454 | } | ||
| 455 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("initTime")) |
| 456 | { | ||
| 457 | 1 | j.at("initTime").get_to(_initTime); | |
| 458 | } | ||
| 459 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (j.contains("initTimeEditFormat")) |
| 460 | { | ||
| 461 | 1 | j.at("initTimeEditFormat").get_to(_initTimeEditFormat); | |
| 462 | } | ||
| 463 | 1 | updatePins(); | |
| 464 | 1 | } | |
| 465 | |||
| 466 | 3 | bool NAV::PosVelAttInitializer::initialize() | |
| 467 | { | ||
| 468 | LOG_TRACE("{}: called", nameId()); | ||
| 469 | |||
| 470 | 3 | _startTime.reset(); | |
| 471 | |||
| 472 | 3 | _countAveragedAttitude = 0.0; | |
| 473 | |||
| 474 | 3 | _lastPositionAccuracy = { std::numeric_limits<double>::infinity(), | |
| 475 | std::numeric_limits<double>::infinity(), | ||
| 476 | std::numeric_limits<double>::infinity() }; | ||
| 477 | 3 | _lastVelocityAccuracy = { std::numeric_limits<double>::infinity(), | |
| 478 | std::numeric_limits<double>::infinity(), | ||
| 479 | std::numeric_limits<double>::infinity() }; | ||
| 480 | |||
| 481 | 3 | _posVelAttInitialized = { false, false, false, false }; | |
| 482 | |||
| 483 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (!_overridePosition || _overrideVelocity == VelocityOverride::OFF |
| 484 |
5/10✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 3 times.
|
6 | || !_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2]) |
| 485 | { | ||
| 486 | ✗ | _initTime = InsTime(InsTime_GPSweekTow(0, 0, 0)); | |
| 487 | } | ||
| 488 | |||
| 489 | 3 | return true; | |
| 490 | } | ||
| 491 | |||
| 492 | 1 | void NAV::PosVelAttInitializer::deinitialize() | |
| 493 | { | ||
| 494 | LOG_TRACE("{}: called", nameId()); | ||
| 495 | 1 | } | |
| 496 | |||
| 497 | 116 | void NAV::PosVelAttInitializer::updatePins() | |
| 498 | { | ||
| 499 |
7/10✓ Branch 1 taken 1 times.
✓ Branch 2 taken 115 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 115 times.
|
116 | if (_overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2] && _inputPinIdxIMU >= 0) |
| 500 | { | ||
| 501 | 1 | nm::DeleteInputPin(inputPins.at(static_cast<size_t>(_inputPinIdxIMU))); | |
| 502 | 1 | _inputPinIdxIMU = -1; | |
| 503 | 1 | _inputPinIdxGNSS--; | |
| 504 | } | ||
| 505 |
3/10✗ Branch 1 not taken.
✓ Branch 2 taken 115 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 115 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 115 times.
✗ Branch 12 not taken.
|
115 | else if ((!_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2]) && _inputPinIdxIMU < 0) |
| 506 | { | ||
| 507 |
4/8✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
345 | nm::CreateInputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }, &PosVelAttInitializer::receiveImuObs, nullptr, 0, 0); |
| 508 | 115 | _inputPinIdxIMU = 0; | |
| 509 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 115 times.
|
115 | if (_inputPinIdxGNSS >= 0) |
| 510 | { | ||
| 511 | ✗ | _inputPinIdxGNSS++; | |
| 512 | } | ||
| 513 | } | ||
| 514 | |||
| 515 | 232 | if (_overridePosition | |
| 516 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | && _overrideVelocity != VelocityOverride::OFF |
| 517 |
8/12✓ Branch 0 taken 1 times.
✓ Branch 1 taken 115 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✓ Branch 14 taken 115 times.
|
117 | && _overrideRollPitchYaw[0] && _overrideRollPitchYaw[1] && _overrideRollPitchYaw[2] && _inputPinIdxGNSS >= 0) |
| 518 | { | ||
| 519 | 1 | nm::DeleteInputPin(inputPins.at(static_cast<size_t>(_inputPinIdxGNSS))); | |
| 520 | 1 | _inputPinIdxGNSS = -1; | |
| 521 | } | ||
| 522 | ✗ | else if ((!_overridePosition || _overrideVelocity == VelocityOverride::OFF | |
| 523 | ✗ | || !_overrideRollPitchYaw[0] || !_overrideRollPitchYaw[1] || !_overrideRollPitchYaw[2]) | |
| 524 |
3/6✗ Branch 0 not taken.
✓ Branch 1 taken 115 times.
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
|
115 | && _inputPinIdxGNSS < 0) |
| 525 | { | ||
| 526 |
4/8✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 575 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
805 | nm::CreateInputPin(this, "PosVelAttInit", Pin::Type::Flow, |
| 527 | { NAV::UbloxObs::type(), NAV::RtklibPosObs::type(), NAV::PosVelAtt::type(), NAV::PosVel::type(), NAV::Pos::type() }, | ||
| 528 | &PosVelAttInitializer::receiveGnssObs, nullptr, 1); | ||
| 529 | 115 | _inputPinIdxGNSS = static_cast<int>(inputPins.size()) - 1; | |
| 530 | } | ||
| 531 | |||
| 532 |
3/4✓ Branch 0 taken 1 times.
✓ Branch 1 taken 115 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
116 | if (_inputPinIdxGNSS < 0 && _inputPinIdxIMU < 0) |
| 533 | { | ||
| 534 | 1 | outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<OutputPin::PollDataFunc>(&PosVelAttInitializer::pollPVASolution); | |
| 535 | } | ||
| 536 | else | ||
| 537 | { | ||
| 538 | 115 | outputPins[OUTPUT_PORT_INDEX_POS_VEL_ATT].data = static_cast<void*>(nullptr); | |
| 539 | } | ||
| 540 | 346 | } | |
| 541 | |||
| 542 | ✗ | void NAV::PosVelAttInitializer::finalizeInit() | |
| 543 | { | ||
| 544 | ✗ | if (!_posVelAttInitialized.at(3) | |
| 545 | ✗ | && (_posVelAttInitialized.at(0) || _overridePosition) | |
| 546 | ✗ | && (_posVelAttInitialized.at(1) || _overrideVelocity != VelocityOverride::OFF) | |
| 547 | ✗ | && (_posVelAttInitialized.at(2) || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)))) | |
| 548 | { | ||
| 549 | ✗ | for (auto& inputPin : inputPins) | |
| 550 | { | ||
| 551 | ✗ | inputPin.queueBlocked = true; | |
| 552 | ✗ | inputPin.queue.clear(); | |
| 553 | } | ||
| 554 | ✗ | _posVelAttInitialized.at(3) = true; | |
| 555 | |||
| 556 | ✗ | if (_overridePosition) | |
| 557 | { | ||
| 558 | ✗ | _e_initPosition = _overridePositionValue.e_position; | |
| 559 | } | ||
| 560 | ✗ | Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(_e_initPosition); | |
| 561 | ✗ | LOG_INFO("{}: Position initialized to Lat {:3.12f} [°], Lon {:3.12f} [°], Alt {:4.3f} [m]", nameId(), | |
| 562 | rad2deg(lla_position.x()), | ||
| 563 | rad2deg(lla_position.y()), | ||
| 564 | lla_position.z()); | ||
| 565 | |||
| 566 | ✗ | if (_overrideVelocity == VelocityOverride::NED) | |
| 567 | { | ||
| 568 | ✗ | _n_initVelocity = _overrideVelocityValues; | |
| 569 | } | ||
| 570 | ✗ | else if (_overrideVelocity == VelocityOverride::ECEF) | |
| 571 | { | ||
| 572 | ✗ | _n_initVelocity = trafo::n_Quat_e(lla_position(0), lla_position(1)) * _overrideVelocityValues; | |
| 573 | } | ||
| 574 | |||
| 575 | ✗ | if (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)) | |
| 576 | { | ||
| 577 | ✗ | _n_Quat_b_init = trafo::n_Quat_b(deg2rad(_overrideRollPitchYawValues.at(0)), | |
| 578 | ✗ | deg2rad(_overrideRollPitchYawValues.at(1)), | |
| 579 | ✗ | deg2rad(_overrideRollPitchYawValues.at(2))); | |
| 580 | } | ||
| 581 | |||
| 582 | ✗ | if (lla_position.z() < 0) | |
| 583 | { | ||
| 584 | ✗ | LOG_WARN("{}: Altitude has a value of {} which is below the ellipsoid height.", nameId(), lla_position.z()); | |
| 585 | } | ||
| 586 | |||
| 587 | ✗ | LOG_INFO("{}: Velocity initialized to v_N {:3.5f} [m/s], v_E {:3.5f} [m/s], v_D {:3.5f} [m/s]", nameId(), | |
| 588 | _n_initVelocity(0), _n_initVelocity(1), _n_initVelocity(2)); | ||
| 589 | |||
| 590 | ✗ | [[maybe_unused]] Eigen::Vector3d rollPitchYaw = trafo::quat2eulerZYX(_n_Quat_b_init); | |
| 591 | ✗ | LOG_INFO("{}: Attitude initialized to Roll {:3.5f} [°], Pitch {:3.5f} [°], Yaw {:3.4f} [°]", nameId(), | |
| 592 | rad2deg(rollPitchYaw.x()), | ||
| 593 | rad2deg(rollPitchYaw.y()), | ||
| 594 | rad2deg(rollPitchYaw.z())); | ||
| 595 | |||
| 596 | ✗ | auto posVelAtt = std::make_shared<PosVelAtt>(); | |
| 597 | ✗ | posVelAtt->insTime = _initTime; | |
| 598 | ✗ | posVelAtt->setPosition_e(_e_initPosition); | |
| 599 | ✗ | posVelAtt->setVelocity_n(_n_initVelocity); | |
| 600 | ✗ | posVelAtt->setAttitude_n_Quat_b(_n_Quat_b_init); | |
| 601 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, posVelAtt); | |
| 602 | ✗ | } | |
| 603 | ✗ | else if (std::ranges::all_of(inputPins, [](const InputPin& inputPin) { | |
| 604 | ✗ | if (auto* connectedPin = inputPin.link.getConnectedPin()) | |
| 605 | { | ||
| 606 | ✗ | return connectedPin->noMoreDataAvailable.load(); | |
| 607 | } | ||
| 608 | ✗ | return !inputPin.isPinLinked(); | |
| 609 | })) | ||
| 610 | { | ||
| 611 | ✗ | LOG_ERROR("{}: State Initialization failed. No more messages incoming to eventually receive a state. Please check which states got initialized and override the others.", nameId()); | |
| 612 | } | ||
| 613 | ✗ | } | |
| 614 | |||
| 615 | ✗ | void NAV::PosVelAttInitializer::receiveImuObs(InputPin::NodeDataQueue& queue, size_t /* pinIdx */) | |
| 616 | { | ||
| 617 | ✗ | auto obs = std::static_pointer_cast<const ImuObs>(queue.extract_front()); | |
| 618 | |||
| 619 | ✗ | if (_posVelAttInitialized.at(3)) { return; } | |
| 620 | LOG_DATA("{}: receiveImuObs at time [{}]", nameId(), obs->insTime.toYMDHMS()); | ||
| 621 | |||
| 622 | ✗ | if (_startTime.empty()) { _startTime = obs->insTime; } | |
| 623 | |||
| 624 | // Position and rotation information for conversion of IMU data from platform to body frame | ||
| 625 | ✗ | const auto& imuPosition = obs->imuPos; | |
| 626 | |||
| 627 | // Choose compenated data if available, otherwise uncompensated | ||
| 628 | ✗ | if (!_overrideRollPitchYaw.at(2) && !obs->p_magneticField.has_value()) | |
| 629 | { | ||
| 630 | ✗ | LOG_ERROR("No magnetometer data available. Please override the Yaw angle."); | |
| 631 | ✗ | return; | |
| 632 | } | ||
| 633 | ✗ | Eigen::Vector3d mag_p = obs->p_magneticField ? obs->p_magneticField.value() : Eigen::Vector3d::Zero(); | |
| 634 | |||
| 635 | // Calculate Magnetic Heading | ||
| 636 | ✗ | const Eigen::Vector3d b_mag = imuPosition.b_quat_p() * mag_p; | |
| 637 | ✗ | auto magneticHeading = -std::atan2(b_mag.y(), b_mag.x()); | |
| 638 | |||
| 639 | // Calculate Roll and Pitch from gravity vector direction (only valid under static conditions) | ||
| 640 | ✗ | const Eigen::Vector3d b_accel = imuPosition.b_quat_p() * obs->p_acceleration * -1; | |
| 641 | ✗ | auto roll = calcRollFromStaticAcceleration(b_accel); | |
| 642 | ✗ | auto pitch = calcPitchFromStaticAcceleration(b_accel); | |
| 643 | |||
| 644 | // TODO: Determine Velocity first and if vehicle not static, initialize the attitude from velocity | ||
| 645 | |||
| 646 | // Average with previous attitude | ||
| 647 | ✗ | _countAveragedAttitude++; | |
| 648 | ✗ | if (_countAveragedAttitude > 1) | |
| 649 | { | ||
| 650 | ✗ | _averagedAttitude.at(0) = (_averagedAttitude.at(0) * (_countAveragedAttitude - 1) + roll) / _countAveragedAttitude; | |
| 651 | ✗ | _averagedAttitude.at(1) = (_averagedAttitude.at(1) * (_countAveragedAttitude - 1) + pitch) / _countAveragedAttitude; | |
| 652 | ✗ | _averagedAttitude.at(2) = (_averagedAttitude.at(2) * (_countAveragedAttitude - 1) + magneticHeading) / _countAveragedAttitude; | |
| 653 | } | ||
| 654 | else | ||
| 655 | { | ||
| 656 | ✗ | _averagedAttitude.at(0) = roll; | |
| 657 | ✗ | _averagedAttitude.at(1) = pitch; | |
| 658 | ✗ | _averagedAttitude.at(2) = magneticHeading; | |
| 659 | } | ||
| 660 | |||
| 661 | ✗ | if ((_attitudeMode == AttitudeMode::BOTH || _attitudeMode == AttitudeMode::IMU | |
| 662 | ✗ | || _inputPinIdxGNSS < 0 || !inputPins.at(static_cast<size_t>(_inputPinIdxGNSS)).isPinLinked()) | |
| 663 | ✗ | && (static_cast<double>((obs->insTime - _startTime).count()) >= _initDuration | |
| 664 | ✗ | || (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)))) | |
| 665 | { | ||
| 666 | ✗ | _n_Quat_b_init = trafo::n_Quat_b(_overrideRollPitchYaw.at(0) ? deg2rad(_overrideRollPitchYawValues.at(0)) : _averagedAttitude.at(0), | |
| 667 | ✗ | _overrideRollPitchYaw.at(1) ? deg2rad(_overrideRollPitchYawValues.at(1)) : _averagedAttitude.at(1), | |
| 668 | ✗ | _overrideRollPitchYaw.at(2) ? deg2rad(_overrideRollPitchYawValues.at(2)) : _averagedAttitude.at(2)); | |
| 669 | |||
| 670 | ✗ | _initTime = obs->insTime; | |
| 671 | ✗ | _posVelAttInitialized.at(2) = true; | |
| 672 | } | ||
| 673 | |||
| 674 | ✗ | finalizeInit(); | |
| 675 | ✗ | } | |
| 676 | |||
| 677 | ✗ | void NAV::PosVelAttInitializer::receiveGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx) | |
| 678 | { | ||
| 679 | ✗ | auto nodeData = queue.extract_front(); | |
| 680 | |||
| 681 | ✗ | if (_posVelAttInitialized.at(3)) { return; } | |
| 682 | LOG_DATA("{}: receiveGnssObs at time [{}]", nameId(), nodeData->insTime.toYMDHMS()); | ||
| 683 | |||
| 684 | ✗ | const auto* sourcePin = inputPins.at(pinIdx).link.getConnectedPin(); | |
| 685 | |||
| 686 | ✗ | if (sourcePin->dataIdentifier.front() == UbloxObs::type()) | |
| 687 | { | ||
| 688 | ✗ | receiveUbloxObs(std::static_pointer_cast<const UbloxObs>(nodeData)); | |
| 689 | } | ||
| 690 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { PosVelAtt::type() })) | |
| 691 | { | ||
| 692 | ✗ | receivePosVelAttObs(std::static_pointer_cast<const PosVelAtt>(nodeData)); | |
| 693 | } | ||
| 694 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { PosVel::type() })) | |
| 695 | { | ||
| 696 | ✗ | receivePosVelObs(std::static_pointer_cast<const PosVel>(nodeData)); | |
| 697 | } | ||
| 698 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(sourcePin->dataIdentifier, { Pos::type() })) | |
| 699 | { | ||
| 700 | ✗ | receivePosObs(std::static_pointer_cast<const Pos>(nodeData)); | |
| 701 | } | ||
| 702 | |||
| 703 | ✗ | finalizeInit(); | |
| 704 | ✗ | } | |
| 705 | |||
| 706 | ✗ | void NAV::PosVelAttInitializer::receiveUbloxObs(const std::shared_ptr<const UbloxObs>& obs) | |
| 707 | { | ||
| 708 | ✗ | if (obs->msgClass == vendor::ublox::UbxClass::UBX_CLASS_NAV) | |
| 709 | { | ||
| 710 | ✗ | auto msgId = static_cast<vendor::ublox::UbxNavMessages>(obs->msgId); | |
| 711 | ✗ | if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_ATT) | |
| 712 | { | ||
| 713 | LOG_DATA("{}: UBX_NAV_ATT: Roll {}, Pitch {}, Heading {} [deg]", nameId(), | ||
| 714 | std::get<vendor::ublox::UbxNavAtt>(obs->data).roll * 1e-5, | ||
| 715 | std::get<vendor::ublox::UbxNavAtt>(obs->data).pitch * 1e-5, | ||
| 716 | std::get<vendor::ublox::UbxNavAtt>(obs->data).heading * 1e-5); | ||
| 717 | } | ||
| 718 | ✗ | else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_POSECEF) | |
| 719 | { | ||
| 720 | ✗ | _lastPositionAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc); | |
| 721 | ✗ | _lastPositionAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc); | |
| 722 | ✗ | _lastPositionAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavPosecef>(obs->data).pAcc); | |
| 723 | |||
| 724 | ✗ | if (_lastPositionAccuracy.at(0) <= _positionAccuracyThreshold | |
| 725 | ✗ | && _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold | |
| 726 | ✗ | && _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold) | |
| 727 | { | ||
| 728 | ✗ | _e_initPosition = Eigen::Vector3d(std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefX * 1e-2, | |
| 729 | ✗ | std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefY * 1e-2, | |
| 730 | ✗ | std::get<vendor::ublox::UbxNavPosecef>(obs->data).ecefZ * 1e-2); | |
| 731 | ✗ | _initTime = obs->insTime; | |
| 732 | |||
| 733 | ✗ | _posVelAttInitialized.at(0) = true; | |
| 734 | } | ||
| 735 | } | ||
| 736 | ✗ | else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_POSLLH) | |
| 737 | { | ||
| 738 | ✗ | _lastPositionAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).hAcc * 1e-1); | |
| 739 | ✗ | _lastPositionAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).hAcc * 1e-1); | |
| 740 | ✗ | _lastPositionAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavPosllh>(obs->data).vAcc * 1e-1); | |
| 741 | |||
| 742 | ✗ | if (_lastPositionAccuracy.at(0) <= _positionAccuracyThreshold | |
| 743 | ✗ | && _lastPositionAccuracy.at(1) <= _positionAccuracyThreshold | |
| 744 | ✗ | && _lastPositionAccuracy.at(2) <= _positionAccuracyThreshold) | |
| 745 | { | ||
| 746 | ✗ | Eigen::Vector3d lla_position(deg2rad(std::get<vendor::ublox::UbxNavPosllh>(obs->data).lat * 1e-7), | |
| 747 | ✗ | deg2rad(std::get<vendor::ublox::UbxNavPosllh>(obs->data).lon * 1e-7), | |
| 748 | ✗ | std::get<vendor::ublox::UbxNavPosllh>(obs->data).height * 1e-3); | |
| 749 | |||
| 750 | ✗ | _e_initPosition = trafo::lla2ecef_WGS84(lla_position); | |
| 751 | ✗ | _initTime = obs->insTime; | |
| 752 | |||
| 753 | ✗ | _posVelAttInitialized.at(0) = true; | |
| 754 | } | ||
| 755 | } | ||
| 756 | ✗ | else if (msgId == vendor::ublox::UbxNavMessages::UBX_NAV_VELNED) | |
| 757 | { | ||
| 758 | ✗ | _lastVelocityAccuracy.at(0) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc); | |
| 759 | ✗ | _lastVelocityAccuracy.at(1) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc); | |
| 760 | ✗ | _lastVelocityAccuracy.at(2) = static_cast<float>(std::get<vendor::ublox::UbxNavVelned>(obs->data).sAcc); | |
| 761 | |||
| 762 | ✗ | if (_lastVelocityAccuracy.at(0) <= _velocityAccuracyThreshold | |
| 763 | ✗ | && _lastVelocityAccuracy.at(1) <= _velocityAccuracyThreshold | |
| 764 | ✗ | && _lastVelocityAccuracy.at(2) <= _velocityAccuracyThreshold) | |
| 765 | { | ||
| 766 | ✗ | _n_initVelocity = Eigen::Vector3d(std::get<vendor::ublox::UbxNavVelned>(obs->data).velN * 1e-2, | |
| 767 | ✗ | std::get<vendor::ublox::UbxNavVelned>(obs->data).velE * 1e-2, | |
| 768 | ✗ | std::get<vendor::ublox::UbxNavVelned>(obs->data).velD * 1e-2); | |
| 769 | ✗ | _initTime = obs->insTime; | |
| 770 | |||
| 771 | ✗ | _posVelAttInitialized.at(1) = true; | |
| 772 | } | ||
| 773 | } | ||
| 774 | } | ||
| 775 | ✗ | } | |
| 776 | |||
| 777 | ✗ | void NAV::PosVelAttInitializer::receivePosObs(const std::shared_ptr<const Pos>& obs) | |
| 778 | { | ||
| 779 | ✗ | _e_initPosition = obs->e_position(); | |
| 780 | ✗ | _initTime = obs->insTime; | |
| 781 | ✗ | _posVelAttInitialized.at(0) = true; | |
| 782 | ✗ | } | |
| 783 | |||
| 784 | ✗ | void NAV::PosVelAttInitializer::receivePosVelObs(const std::shared_ptr<const PosVel>& obs) | |
| 785 | { | ||
| 786 | ✗ | receivePosObs(obs); | |
| 787 | |||
| 788 | ✗ | _n_initVelocity = obs->n_velocity(); | |
| 789 | ✗ | _initTime = obs->insTime; | |
| 790 | ✗ | _posVelAttInitialized.at(1) = true; | |
| 791 | ✗ | } | |
| 792 | |||
| 793 | ✗ | void NAV::PosVelAttInitializer::receivePosVelAttObs(const std::shared_ptr<const PosVelAtt>& obs) | |
| 794 | { | ||
| 795 | ✗ | receivePosVelObs(obs); | |
| 796 | |||
| 797 | ✗ | if (_attitudeMode == AttitudeMode::BOTH || _attitudeMode == AttitudeMode::GNSS | |
| 798 | ✗ | || _inputPinIdxIMU < 0 || !inputPins.at(static_cast<size_t>(_inputPinIdxIMU)).isPinLinked()) | |
| 799 | { | ||
| 800 | ✗ | if (_overrideRollPitchYaw.at(0) || _overrideRollPitchYaw.at(1) || _overrideRollPitchYaw.at(2)) | |
| 801 | { | ||
| 802 | ✗ | const Eigen::Vector3d rollPitchYaw = obs->rollPitchYaw(); | |
| 803 | |||
| 804 | ✗ | _n_Quat_b_init = trafo::n_Quat_b(_overrideRollPitchYaw.at(0) ? deg2rad(_overrideRollPitchYawValues.at(0)) : rollPitchYaw(0), | |
| 805 | ✗ | _overrideRollPitchYaw.at(1) ? deg2rad(_overrideRollPitchYawValues.at(1)) : rollPitchYaw(1), | |
| 806 | ✗ | _overrideRollPitchYaw.at(2) ? deg2rad(_overrideRollPitchYawValues.at(2)) : rollPitchYaw(2)); | |
| 807 | } | ||
| 808 | else | ||
| 809 | { | ||
| 810 | ✗ | _n_Quat_b_init = obs->n_Quat_b(); | |
| 811 | } | ||
| 812 | ✗ | _initTime = obs->insTime; | |
| 813 | |||
| 814 | ✗ | _posVelAttInitialized.at(2) = true; | |
| 815 | } | ||
| 816 | ✗ | } | |
| 817 | |||
| 818 | 2 | std::shared_ptr<const NAV::NodeData> NAV::PosVelAttInitializer::pollPVASolution() | |
| 819 | { | ||
| 820 |
2/4✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
|
2 | if (_inputPinIdxIMU >= 0 || _inputPinIdxGNSS >= 0) |
| 821 | { | ||
| 822 | ✗ | return nullptr; | |
| 823 | } | ||
| 824 | |||
| 825 | 2 | int initCount = 0; | |
| 826 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if (_overridePosition) |
| 827 | { | ||
| 828 | 2 | _e_initPosition = _overridePositionValue.e_position; | |
| 829 | 2 | ++initCount; | |
| 830 | } | ||
| 831 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if (_overrideVelocity == VelocityOverride::NED) |
| 832 | { | ||
| 833 | 2 | _n_initVelocity = _overrideVelocityValues; | |
| 834 | 2 | ++initCount; | |
| 835 | } | ||
| 836 | ✗ | else if (_overrideVelocity == VelocityOverride::ECEF) | |
| 837 | { | ||
| 838 | ✗ | Eigen::Vector3d pos_lla = trafo::ecef2lla_WGS84(_e_initPosition); | |
| 839 | ✗ | _n_initVelocity = trafo::n_Quat_e(pos_lla(0), pos_lla(1)) * _overrideVelocityValues; | |
| 840 | ✗ | ++initCount; | |
| 841 | } | ||
| 842 | |||
| 843 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
|
2 | if (_overrideRollPitchYaw.at(0) && _overrideRollPitchYaw.at(1) && _overrideRollPitchYaw.at(2)) |
| 844 | { | ||
| 845 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | _n_Quat_b_init = trafo::n_Quat_b(deg2rad(_overrideRollPitchYawValues.at(0)), |
| 846 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | deg2rad(_overrideRollPitchYawValues.at(1)), |
| 847 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | deg2rad(_overrideRollPitchYawValues.at(2))); |
| 848 | 2 | ++initCount; | |
| 849 | } | ||
| 850 |
5/6✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 1 times.
|
2 | if (initCount == 3 && !_posVelAttInitialized.at(3)) |
| 851 | { | ||
| 852 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | auto posVelAtt = std::make_shared<PosVelAtt>(); |
| 853 | 1 | posVelAtt->insTime = _initTime; | |
| 854 | |||
| 855 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _posVelAttInitialized.at(3) = true; |
| 856 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | posVelAtt->setPosition_e(_e_initPosition); |
| 857 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | posVelAtt->setVelocity_n(_n_initVelocity); |
| 858 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | posVelAtt->setAttitude_n_Quat_b(_n_Quat_b_init); |
| 859 | |||
| 860 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, posVelAtt); |
| 861 | 1 | return posVelAtt; | |
| 862 | 1 | } | |
| 863 | 1 | return nullptr; | |
| 864 | } | ||
| 865 |