INSTINCT Code Coverage Report

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