INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/GNSS/SinglePointPositioning.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 42 80 52.5%
Functions: 10 15 66.7%
Branches: 42 162 25.9%
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 "SinglePointPositioning.hpp"
10 #include <imgui.h>
11
12 #include "Navigation/GNSS/Positioning/SPP/Algorithm.hpp"
13 #include "NodeData/State/PosVel.hpp"
14 #include "internal/NodeManager.hpp"
15 #include <fmt/core.h>
16 namespace nm = NAV::NodeManager;
17 #include "internal/FlowManager.hpp"
18 #include "internal/gui/NodeEditorApplication.hpp"
19 #include "internal/gui/widgets/imgui_ex.hpp"
20
21 #include "Navigation/Constants.hpp"
22
23 #include "NodeData/GNSS/GnssObs.hpp"
24 #include "NodeData/GNSS/GnssNavInfo.hpp"
25 #include "NodeData/GNSS/SppSolution.hpp"
26
27 #include "util/Logger.hpp"
28 #include "util/Container/Vector.hpp"
29
30 120 NAV::SinglePointPositioning::SinglePointPositioning()
31
4/8
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 120 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 120 times.
✗ Branch 14 not taken.
 120 : Node(typeStatic())
32 {
33 LOG_TRACE("{}: called", name);
34
35 120 _hasConfig = true;
36 120 _guiConfigDefaultWindowSize = { 538, 536 };
37
38
5/10
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 120 times.
✓ Branch 14 taken 120 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 360 nm::CreateInputPin(this, NAV::GnssObs::type().c_str(), Pin::Type::Flow, { NAV::GnssObs::type() }, &SinglePointPositioning::recvGnssObs);
39
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _dynamicInputPins.addPin(this); // GnssNavInfo
40
41
5/10
✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 120 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 120 times.
✓ Branch 15 taken 120 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 480 nm::CreateOutputPin(this, NAV::SppSolution::type().c_str(), Pin::Type::Flow, { NAV::SppSolution::type() });
42 360 }
43
44 256 NAV::SinglePointPositioning::~SinglePointPositioning()
45 {
46 LOG_TRACE("{}: called", nameId());
47 256 }
48
49 232 std::string NAV::SinglePointPositioning::typeStatic()
50 {
51
1/2
✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
 464 return "SinglePointPositioning - SPP";
52 }
53
54 ✗ std::string NAV::SinglePointPositioning::type() const
55 {
56 ✗ return typeStatic();
57 }
58
59 112 std::string NAV::SinglePointPositioning::category()
60 {
61
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Data Processor";
62 }
63
64 ✗ void NAV::SinglePointPositioning::guiConfig()
65 {
66 ✗ auto nSatColumnContent = [&](size_t pinIndex) -> bool {
67 ✗ if (auto gnssNavInfo = getInputValue<GnssNavInfo>(pinIndex))
68 {
69 ✗ size_t usedSatNum = 0;
70 ✗ std::string usedSats;
71 ✗ std::string allSats;
72
73 ✗ std::string filler = ", ";
74 ✗ for (const auto& satellite : gnssNavInfo->v->satellites())
75 {
76 ✗ if (_algorithm._obsFilter.isSatelliteAllowed(satellite.first))
77 {
78 ✗ usedSatNum++;
79 ✗ usedSats += (allSats.empty() ? "" : filler) + fmt::format("{}", satellite.first);
80 }
81 ✗ allSats += (allSats.empty() ? "" : filler) + fmt::format("{}", satellite.first);
82 }
83 ✗ ImGui::TextUnformatted(fmt::format("{} / {}", usedSatNum, gnssNavInfo->v->nSatellites()).c_str());
84 ✗ if (ImGui::IsItemHovered())
85 {
86 ✗ ImGui::SetTooltip("Used satellites: %s\n"
87 "Avail satellites: %s",
88 usedSats.c_str(), allSats.c_str());
89 }
90 ✗ }
91 ✗ return false;
92 ✗ };
93
94 ✗ if (_dynamicInputPins.ShowGuiWidgets(size_t(id), inputPins, this, { { .header = "# Sat", .content = nSatColumnContent } }))
95 {
96 ✗ flow::ApplyChanges();
97 }
98
99 ✗ ImGui::Separator();
100
101 // ###########################################################################################################
102
103 ✗ const float itemWidth = 280.0F * gui::NodeEditorApplication::windowFontRatio();
104 ✗ const float unitWidth = 100.0F * gui::NodeEditorApplication::windowFontRatio();
105
106 ✗ if (_algorithm.ShowGuiWidgets(nameId().c_str(), itemWidth, unitWidth))
107 {
108 ✗ flow::ApplyChanges();
109 }
110 ✗ }
111
112 ✗ [[nodiscard]] json NAV::SinglePointPositioning::save() const
113 {
114 LOG_TRACE("{}: called", nameId());
115
116 return {
117 ✗ { "dynamicInputPins", _dynamicInputPins },
118 ✗ { "algorithm", _algorithm },
119 ✗ };
120 ✗ }
121
122 8 void NAV::SinglePointPositioning::restore(json const& j)
123 {
124 LOG_TRACE("{}: called", nameId());
125
126
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("dynamicInputPins")) { NAV::gui::widgets::from_json(j.at("dynamicInputPins"), _dynamicInputPins, this); }
127
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("algorithm")) { j.at("algorithm").get_to(_algorithm); }
128 8 }
129
130 24 bool NAV::SinglePointPositioning::initialize()
131 {
132 LOG_TRACE("{}: called", nameId());
133
134
2/4
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 24 times.
 48 if (std::all_of(inputPins.begin() + INPUT_PORT_INDEX_GNSS_NAV_INFO, inputPins.end(), [](const InputPin& inputPin) { return !inputPin.isPinLinked(); }))
135 {
136 ✗ LOG_ERROR("{}: You need to connect a GNSS NavigationInfo provider", nameId());
137 ✗ return false;
138 }
139
140 24 _algorithm.reset();
141
142
2/4
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
 24 LOG_DEBUG("{}: initialized", nameId());
143
144 24 return true;
145 }
146
147 8 void NAV::SinglePointPositioning::deinitialize()
148 {
149 LOG_TRACE("{}: called", nameId());
150 8 }
151
152 122 void NAV::SinglePointPositioning::pinAddCallback(Node* node)
153 {
154
5/10
✓ Branch 2 taken 122 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 122 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 122 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 122 times.
✓ Branch 15 taken 122 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 488 nm::CreateInputPin(node, NAV::GnssNavInfo::type().c_str(), Pin::Type::Object, { NAV::GnssNavInfo::type() });
155 244 }
156
157 ✗ void NAV::SinglePointPositioning::pinDeleteCallback(Node* node, size_t pinIdx)
158 {
159 ✗ nm::DeleteInputPin(node->inputPins.at(pinIdx));
160 ✗ }
161
162 407 void NAV::SinglePointPositioning::recvGnssObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
163 {
164 // Collection of all connected navigation data providers
165 407 std::vector<InputPin::IncomingLink::ValueWrapper<GnssNavInfo>> gnssNavInfoWrappers;
166 407 std::vector<const GnssNavInfo*> gnssNavInfos;
167
3/4
✓ Branch 1 taken 912 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 505 times.
✓ Branch 4 taken 407 times.
 912 for (size_t i = 0; i < _dynamicInputPins.getNumberOfDynamicPins(); i++)
168 {
169
2/4
✓ Branch 1 taken 505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 505 times.
✗ Branch 5 not taken.
 505 if (auto gnssNavInfo = getInputValue<GnssNavInfo>(INPUT_PORT_INDEX_GNSS_NAV_INFO + i))
170 {
171
1/2
✓ Branch 2 taken 505 times.
✗ Branch 3 not taken.
 505 gnssNavInfoWrappers.push_back(*gnssNavInfo);
172
1/2
✓ Branch 2 taken 505 times.
✗ Branch 3 not taken.
 505 gnssNavInfos.push_back(gnssNavInfo->v);
173 505 }
174 }
175
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 407 times.
 407 if (gnssNavInfos.empty()) { return; }
176
177
1/2
✓ Branch 1 taken 407 times.
✗ Branch 2 not taken.
 407 auto gnssObs = std::static_pointer_cast<const GnssObs>(queue.extract_front());
178 LOG_DATA("{}: [{}] Calculating SPP", nameId(), gnssObs->insTime.toYMDHMS(GPST));
179
180
4/6
✓ Branch 1 taken 407 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 407 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 406 times.
✓ Branch 9 taken 1 times.
 407 if (auto sppSol = _algorithm.calcSppSolution(gnssObs, gnssNavInfos, nameId()))
181 {
182
1/2
✓ Branch 2 taken 406 times.
✗ Branch 3 not taken.
 406 invokeCallbacks(OUTPUT_PORT_INDEX_SPPSOL, sppSol);
183 407 }
184 407 }
185