INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Converter/GNSS/UartPacketConverter.cpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 13 137 9.5%
Functions: 4 10 40.0%
Branches: 12 289 4.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 "UartPacketConverter.hpp"
10
11 #include <memory>
12
13 #include "util/Logger.hpp"
14 #include "util/Time/TimeBase.hpp"
15
16 #include "internal/NodeManager.hpp"
17 namespace nm = NAV::NodeManager;
18 #include "internal/FlowManager.hpp"
19
20 #include "util/Vendor/Ublox/UbloxUtilities.hpp"
21 #include "util/Vendor/Emlid/EmlidUtilities.hpp"
22 #include "util/Vendor/Espressif/EspressifUtilities.hpp"
23
24 #include "Nodes/DataProvider/IMU/Sensors/VectorNavSensor.hpp"
25
26 #include "NodeData/General/UartPacket.hpp"
27 #include "NodeData/GNSS/UbloxObs.hpp"
28 #include "NodeData/GNSS/EmlidObs.hpp"
29 #include "NodeData/WiFi/WiFiObs.hpp"
30
31 112 NAV::UartPacketConverter::UartPacketConverter()
32
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 112 : Node(typeStatic())
33 {
34 LOG_TRACE("{}: called", name);
35 112 _hasConfig = true;
36 112 _guiConfigDefaultWindowSize = { 340, 93 };
37
38
4/8
✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 112 times.
✓ Branch 10 taken 112 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 448 nm::CreateOutputPin(this, "UbloxObs", Pin::Type::Flow, { NAV::UbloxObs::type() });
39
40
4/8
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112 times.
✓ Branch 9 taken 112 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 336 nm::CreateInputPin(this, "UartPacket", Pin::Type::Flow, { NAV::UartPacket::type() }, &UartPacketConverter::receiveObs);
41 336 }
42
43 224 NAV::UartPacketConverter::~UartPacketConverter()
44 {
45 LOG_TRACE("{}: called", nameId());
46 224 }
47
48 224 std::string NAV::UartPacketConverter::typeStatic()
49 {
50
1/2
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
 448 return "UartPacketConverter";
51 }
52
53 ✗ std::string NAV::UartPacketConverter::type() const
54 {
55 ✗ return typeStatic();
56 }
57
58 112 std::string NAV::UartPacketConverter::category()
59 {
60
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Converter";
61 }
62
63 ✗ void NAV::UartPacketConverter::guiConfig()
64 {
65 ✗ if (auto outputType = static_cast<int>(_outputType);
66 ✗ ImGui::Combo(fmt::format("Output Type##{}", size_t(id)).c_str(), &outputType, "UbloxObs\0EmlidObs\0WiFiObs\0\0"))
67 {
68 ✗ _outputType = static_cast<decltype(_outputType)>(outputType);
69 ✗ std::string outputTypeString;
70 ✗ switch (_outputType)
71 {
72 ✗ case OutputType_UbloxObs:
73 ✗ outputTypeString = "UbloxObs";
74 ✗ break;
75 ✗ case OutputType_EmlidObs:
76 ✗ outputTypeString = "EmlidObs";
77 ✗ break;
78 ✗ default:
79 ✗ outputTypeString = "WiFiObs";
80 }
81
82 ✗ LOG_DEBUG("{}: Output Type changed to {}", nameId(), outputTypeString);
83
84 ✗ if (_outputType == OutputType_UbloxObs)
85 {
86 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::UbloxObs::type() };
87 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::UbloxObs::type();
88 }
89 ✗ else if (_outputType == OutputType_EmlidObs)
90 {
91 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EmlidObs::type() };
92 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EmlidObs::type();
93 }
94 ✗ else if (_outputType == OutputType_WiFiObs)
95 {
96 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::WiFiObs::type() };
97 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::WiFiObs::type();
98 }
99
100 ✗ for (auto& link : outputPins.front().links)
101 {
102 ✗ if (auto* connectedPin = link.getConnectedPin())
103 {
104 ✗ outputPins.front().recreateLink(*connectedPin);
105 }
106 }
107 ✗ flow::ApplyChanges();
108 ✗ }
109
110 ✗ if (_outputType == OutputType_WiFiObs)
111 {
112 ✗ if (ImGui::Checkbox(fmt::format("Show SyncIn Pin##{}", size_t(id)).c_str(), &_syncInPin))
113 {
114 ✗ LOG_DEBUG("{}: syncInPin changed to {}", nameId(), _syncInPin);
115 ✗ flow::ApplyChanges();
116 ✗ if (_syncInPin && (inputPins.size() <= 1))
117 {
118 ✗ nm::CreateInputPin(this, "SyncIn", Pin::Type::Object, { "TimeSync" });
119 }
120 ✗ else if (!_syncInPin)
121 {
122 ✗ nm::DeleteInputPin(inputPins.at(INPUT_PORT_INDEX_SYNC_IN));
123 }
124 }
125 // Remove the extra flow::ApplyChanges() statement here
126 }
127 ✗ else if (inputPins.size() > 1)
128 {
129 ✗ nm::DeleteInputPin(inputPins.at(INPUT_PORT_INDEX_SYNC_IN));
130 }
131 ✗ }
132
133 ✗ [[nodiscard]] json NAV::UartPacketConverter::save() const
134 {
135 LOG_TRACE("{}: called", nameId());
136
137 ✗ json j;
138
139 ✗ j["outputType"] = _outputType;
140 ✗ j["syncInPin"] = _syncInPin;
141
142 ✗ return j;
143 ✗ }
144
145 ✗ void NAV::UartPacketConverter::restore(json const& j)
146 {
147 LOG_TRACE("{}: called", nameId());
148
149 ✗ if (j.contains("outputType"))
150 {
151 ✗ j.at("outputType").get_to(_outputType);
152
153 ✗ if (!outputPins.empty())
154 {
155 ✗ if (_outputType == OutputType_UbloxObs)
156 {
157 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::UbloxObs::type() };
158 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::UbloxObs::type();
159 }
160 ✗ else if (_outputType == OutputType_EmlidObs)
161 {
162 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EmlidObs::type() };
163 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EmlidObs::type();
164 }
165 ✗ else if (_outputType == OutputType_WiFiObs)
166 {
167 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::WiFiObs::type() };
168 ✗ outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::WiFiObs::type();
169 }
170 }
171 }
172 ✗ if (j.contains("syncInPin") && _outputType == OutputType_WiFiObs)
173 {
174 ✗ j.at("syncInPin").get_to(_syncInPin);
175 ✗ if (_syncInPin && inputPins.size() <= 1)
176 {
177 ✗ nm::CreateInputPin(this, "SyncIn", Pin::Type::Object, { "TimeSync" });
178 }
179 }
180 ✗ }
181
182 ✗ bool NAV::UartPacketConverter::initialize()
183 {
184 LOG_TRACE("{}: called", nameId());
185
186 ✗ _lastSyncInCnt = 0;
187 ✗ _lastSyncOutCnt = 0;
188 ✗ _syncOutCntCorr = 0;
189 ✗ _syncInCntCorr = 0;
190
191 ✗ return true;
192 }
193
194 ✗ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
195 {
196 ✗ auto uartPacket = std::static_pointer_cast<const UartPacket>(queue.extract_front());
197
198 ✗ std::shared_ptr<NodeData> convertedData = nullptr;
199 ✗ if (_outputType == OutputType_UbloxObs)
200 {
201 ✗ auto obs = std::make_shared<UbloxObs>();
202 ✗ auto packet = uartPacket->raw;
203 ✗ if (!vendor::ublox::decryptUbloxObs(obs, packet, nameId())) { return; };
204 ✗ convertedData = obs;
205 ✗ }
206 ✗ else if (_outputType == OutputType_WiFiObs)
207 {
208 ✗ auto obs = std::make_shared<WiFiObs>();
209 ✗ auto packet = uartPacket->raw;
210
211 ✗ if (!vendor::espressif::decryptWiFiObs(obs, packet, nameId()))
212 {
213 ✗ LOG_WARN("{}: Decoding WiFi Obs failed", nameId());
214 ✗ return;
215 };
216 ✗ if (_syncInPin && inputPins.at(INPUT_PORT_INDEX_SYNC_IN).isPinLinked())
217 {
218 ✗ if (auto timeSyncMaster = getInputValue<const NAV::VectorNavSensor::TimeSync>(INPUT_PORT_INDEX_SYNC_IN);
219 ✗ timeSyncMaster && !timeSyncMaster->v->ppsTime.empty())
220 {
221 ✗ if (_lastSyncInCnt > obs->timeOutputs.syncInCnt) // reset of the slave
222 {
223 ✗ LOG_DEBUG("{}: Slave reset during time sync", nameId());
224 ✗ _syncOutCntCorr = 0;
225 ✗ _syncInCntCorr = timeSyncMaster->v->syncOutCnt;
226 }
227 ✗ else if (_lastSyncOutCnt > timeSyncMaster->v->syncOutCnt) // reset of the master
228 {
229 ✗ LOG_DEBUG("{}: Master reset during time sync", nameId());
230 ✗ _syncInCntCorr = 0;
231 ✗ _syncOutCntCorr = obs->timeOutputs.syncInCnt;
232 }
233 ✗ else if (_lastSyncOutCnt == 0 && timeSyncMaster->v->syncOutCnt > 1) // slave counter started later
234 {
235 ✗ LOG_DEBUG("{}: Slave counter started later", nameId());
236 ✗ _syncInCntCorr = timeSyncMaster->v->syncOutCnt;
237 }
238 ✗ else if (_lastSyncOutCnt == 0 && obs->timeOutputs.syncInCnt > 1) // master counter started later
239 {
240 ✗ LOG_DEBUG("{}: Master counter started later", nameId());
241 ✗ _syncOutCntCorr = obs->timeOutputs.syncInCnt;
242 }
243 ✗ _lastSyncOutCnt = timeSyncMaster->v->syncOutCnt;
244 ✗ _lastSyncInCnt = obs->timeOutputs.syncInCnt;
245 ✗ int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->v->syncOutCnt - _syncOutCntCorr;
246 ✗ obs->insTime = timeSyncMaster->v->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn)
247 ✗ + std::chrono::seconds(syncCntDiff);
248 // LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}, syncInCntCorr {}, syncOutCntCorr {}",
249 // nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(),
250 // timeSyncMaster->syncOutCnt, obs->timeOutputs.syncInCnt, syncCntDiff, _syncInCntCorr, _syncOutCntCorr);
251 }
252 else
253 {
254 ✗ return;
255 ✗ }
256 }
257 ✗ convertedData = obs;
258 ✗ }
259 else /* if (_outputType == OutputType_EmlidObs) */
260 {
261 ✗ auto obs = std::make_shared<EmlidObs>();
262 ✗ auto packet = uartPacket->raw;
263 ✗ vendor::emlid::decryptEmlidObs(obs, packet);
264 ✗ convertedData = obs;
265 ✗ }
266
267 ✗ if (auto currentTime = util::time::GetCurrentInsTime();
268 ✗ !currentTime.empty())
269 {
270 ✗ convertedData->insTime = currentTime;
271 }
272
273 ✗ if (convertedData)
274 {
275 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_CONVERTED, convertedData);
276 }
277 ✗ }
278