INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/Integrator/ImuIntegrator.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 48 71 67.6%
Functions: 10 13 76.9%
Branches: 44 156 28.2%
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 "ImuIntegrator.hpp"
10
11 #include "util/Logger.hpp"
12
13 #include "NodeData/IMU/ImuObs.hpp"
14 #include "NodeData/IMU/ImuObsWDelta.hpp"
15 #include "NodeData/State/PosVelAtt.hpp"
16
17 #include "Navigation/Constants.hpp"
18 #include "Navigation/Math/Math.hpp"
19
20 #include "internal/gui/widgets/HelpMarker.hpp"
21 #include <imgui_internal.h>
22
23 #include "util/Eigen.hpp"
24
25 #include "NodeRegistry.hpp"
26 #include "internal/NodeManager.hpp"
27 namespace nm = NAV::NodeManager;
28 #include "internal/FlowManager.hpp"
29
30 #include <algorithm>
31
32 114 NAV::ImuIntegrator::ImuIntegrator()
33
3/6
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 114 times.
✗ Branch 9 not taken.
 114 : Node(typeStatic())
34 {
35 LOG_TRACE("{}: called", name);
36
37 114 _hasConfig = true;
38 114 _guiConfigDefaultWindowSize = { 422, 146 };
39
40
4/8
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 228 times.
✓ Branch 10 taken 114 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 570 nm::CreateInputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type(), NAV::ImuObsWDelta::type() }, &ImuIntegrator::recvObservation,
41 ✗ [](const Node* node, const InputPin& inputPin) {
42 786 const auto* imuIntegrator = static_cast<const ImuIntegrator*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
43
2/4
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
 786 return !inputPin.queue.empty() && imuIntegrator->_inertialIntegrator.hasInitialPosition();
44 });
45
4/8
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 114 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 114 times.
✓ Branch 9 taken 114 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 342 nm::CreateInputPin(this, "PosVelAttInit", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &ImuIntegrator::recvPosVelAttInit, nullptr, 1);
46
47
4/8
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 114 times.
✓ Branch 10 taken 114 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 456 nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
48 456 }
49
50 232 NAV::ImuIntegrator::~ImuIntegrator()
51 {
52 LOG_TRACE("{}: called", nameId());
53 232 }
54
55 226 std::string NAV::ImuIntegrator::typeStatic()
56 {
57
1/2
✓ Branch 1 taken 226 times.
✗ Branch 2 not taken.
 452 return "ImuIntegrator";
58 }
59
60 ✗ std::string NAV::ImuIntegrator::type() const
61 {
62 ✗ return typeStatic();
63 }
64
65 112 std::string NAV::ImuIntegrator::category()
66 {
67
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Data Processor";
68 }
69
70 ✗ void NAV::ImuIntegrator::guiConfig()
71 {
72 ✗ if (InertialIntegratorGui(std::to_string(size_t(id)).c_str(), _inertialIntegrator))
73 {
74 ✗ flow::ApplyChanges();
75 }
76 ✗ if (inputPins.at(INPUT_PORT_INDEX_IMU_OBS).isPinLinked()
77 ✗ && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU_OBS).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
78 {
79 ✗ ImGui::Separator();
80 ✗ if (ImGui::Checkbox(fmt::format("Prefer raw measurements over delta##{}", size_t(id)).c_str(), &_preferAccelerationOverDeltaMeasurements))
81 {
82 ✗ flow::ApplyChanges();
83 }
84 }
85 ✗ }
86
87 ✗ [[nodiscard]] json NAV::ImuIntegrator::save() const
88 {
89 LOG_TRACE("{}: called", nameId());
90
91 ✗ json j;
92
93 ✗ j["inertialIntegrator"] = _inertialIntegrator;
94 ✗ j["preferAccelerationOverDeltaMeasurements"] = _preferAccelerationOverDeltaMeasurements;
95
96 ✗ return j;
97 ✗ }
98
99 2 void NAV::ImuIntegrator::restore(json const& j)
100 {
101 LOG_TRACE("{}: called", nameId());
102
103
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("inertialIntegrator"))
104 {
105 2 j.at("inertialIntegrator").get_to(_inertialIntegrator);
106 }
107
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("preferAccelerationOverDeltaMeasurements"))
108 {
109 2 j.at("preferAccelerationOverDeltaMeasurements").get_to(_preferAccelerationOverDeltaMeasurements);
110 }
111 2 }
112
113 3 bool NAV::ImuIntegrator::initialize()
114 {
115 LOG_TRACE("{}: called", nameId());
116
117 3 _inertialIntegrator.reset();
118
119
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 LOG_DEBUG("ImuIntegrator initialized");
120
121 3 return true;
122 }
123
124 1 void NAV::ImuIntegrator::deinitialize()
125 {
126 LOG_TRACE("{}: called", nameId());
127 1 }
128
129 786 void NAV::ImuIntegrator::recvObservation(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
130 {
131
1/2
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
 786 auto nodeData = queue.extract_front();
132
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 786 times.
 786 if (nodeData->insTime.empty())
133 {
134 ✗ LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime)", nameId());
135 ✗ return;
136 }
137
138 786 std::shared_ptr<NAV::PosVelAtt> integratedPosVelAtt = nullptr;
139
140 1572 if (!_preferAccelerationOverDeltaMeasurements
141
12/30
✓ Branch 0 taken 786 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 786 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 786 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 786 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 786 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 786 times.
✗ Branch 15 not taken.
✓ Branch 16 taken 786 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 786 times.
✗ Branch 20 not taken.
✓ Branch 21 taken 786 times.
✗ Branch 22 not taken.
✓ Branch 23 taken 786 times.
✓ Branch 24 taken 786 times.
✓ Branch 26 taken 786 times.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
 3144 && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU_OBS).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
142 {
143 786 auto obs = std::static_pointer_cast<const ImuObsWDelta>(nodeData);
144 LOG_DATA("{}: recvImuObsWDelta at time [{}]", nameId(), obs->insTime.toYMDHMS());
145
146
1/2
✓ Branch 6 taken 786 times.
✗ Branch 7 not taken.
 786 integratedPosVelAtt = _inertialIntegrator.calcInertialSolutionDelta(obs->insTime, obs->dtime, obs->dvel, obs->dtheta, obs->imuPos);
147 786 }
148 else
149 {
150 ✗ auto obs = std::static_pointer_cast<const ImuObs>(nodeData);
151 LOG_DATA("{}: recvImuObs at time [{}]", nameId(), obs->insTime.toYMDHMS());
152
153 ✗ integratedPosVelAtt = _inertialIntegrator.calcInertialSolution(obs->insTime, obs->p_acceleration, obs->p_angularRate, obs->imuPos);
154 ✗ }
155
156
1/2
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
 786 if (integratedPosVelAtt)
157 {
158 LOG_DATA("{}: e_position = {}", nameId(), integratedPosVelAtt->e_position().transpose());
159 LOG_DATA("{}: e_velocity = {}", nameId(), integratedPosVelAtt->e_velocity().transpose());
160 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(integratedPosVelAtt->rollPitchYaw()).transpose());
161
1/2
✓ Branch 2 taken 786 times.
✗ Branch 3 not taken.
 786 invokeCallbacks(OUTPUT_PORT_INDEX_INERTIAL_NAV_SOL, integratedPosVelAtt);
162 }
163 1572 }
164
165 1 void NAV::ImuIntegrator::recvPosVelAttInit(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
166 {
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 auto posVelAtt = std::static_pointer_cast<const PosVelAtt>(queue.extract_front());
168 1 inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queueBlocked = true;
169 1 inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queue.clear();
170
171 LOG_DATA("{}: recvPosVelAttInit at time [{}]", nameId(), posVelAtt->insTime.toYMDHMS());
172
173
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (!_inertialIntegrator.hasInitialPosition())
174 {
175
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 _inertialIntegrator.setInitialState(*posVelAtt);
176 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt->e_position().transpose());
177 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt->e_velocity().transpose());
178 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt->rollPitchYaw()).transpose());
179
180
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 invokeCallbacks(OUTPUT_PORT_INDEX_INERTIAL_NAV_SOL, posVelAtt);
181 }
182 1 }
183