INSTINCT Code Coverage Report

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