INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Converter/GNSS/UartPacketConverter.cpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 13 112 11.6%
Functions: 4 10 40.0%
Branches: 12 227 5.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 "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 114 NAV::UartPacketConverter::UartPacketConverter()
32
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 : Node(typeStatic())
33 {
34 LOG_TRACE("{}: called", name);
35 114 _hasConfig = true;
36 114 _guiConfigDefaultWindowSize = { 340, 93 };
37
38
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, "UbloxObs", Pin::Type::Flow, { NAV::UbloxObs::type() });
39
40
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, "UartPacket", Pin::Type::Flow, { NAV::UartPacket::type() }, &UartPacketConverter::receiveObs);
41 342 }
42
43 228 NAV::UartPacketConverter::~UartPacketConverter()
44 {
45 LOG_TRACE("{}: called", nameId());
46 228 }
47
48 228 std::string NAV::UartPacketConverter::typeStatic()
49 {
50
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 456 return "UartPacketConverter";
51 }
52
53 ✗ std::string NAV::UartPacketConverter::type() const
54 {
55 ✗ return typeStatic();
56 }
57
58 114 std::string NAV::UartPacketConverter::category()
59 {
60
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 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 // TODO: This code does not work currently. Needs debugging
217 // if (_syncInPin && inputPins.at(INPUT_PORT_INDEX_SYNC_IN).isPinLinked())
218 // {
219 // if (auto timeSyncMaster = getInputValue<const NAV::VectorNavSensor::TimeSync>(INPUT_PORT_INDEX_SYNC_IN);
220 // timeSyncMaster && !timeSyncMaster->v->ppsTime.empty())
221 // {
222 // if (_lastSyncInCnt > obs->timeOutputs.syncInCnt) // reset of the slave
223 // {
224 // LOG_DEBUG("{}: Slave reset during time sync", nameId());
225 // _syncOutCntCorr = 0;
226 // _syncInCntCorr = timeSyncMaster->v->syncOutCnt;
227 // }
228 // else if (_lastSyncOutCnt > timeSyncMaster->v->syncOutCnt) // reset of the master
229 // {
230 // LOG_DEBUG("{}: Master reset during time sync", nameId());
231 // _syncInCntCorr = 0;
232 // _syncOutCntCorr = obs->timeOutputs.syncInCnt;
233 // }
234 // else if (_lastSyncOutCnt == 0 && timeSyncMaster->v->syncOutCnt > 1) // slave counter started later
235 // {
236 // LOG_DEBUG("{}: Slave counter started later", nameId());
237 // _syncInCntCorr = timeSyncMaster->v->syncOutCnt;
238 // }
239 // else if (_lastSyncOutCnt == 0 && obs->timeOutputs.syncInCnt > 1) // master counter started later
240 // {
241 // LOG_DEBUG("{}: Master counter started later", nameId());
242 // _syncOutCntCorr = obs->timeOutputs.syncInCnt;
243 // }
244 // _lastSyncOutCnt = timeSyncMaster->v->syncOutCnt;
245 // _lastSyncInCnt = obs->timeOutputs.syncInCnt;
246 // int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->v->syncOutCnt - _syncOutCntCorr;
247 // obs->insTime = timeSyncMaster->v->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn)
248 // + std::chrono::seconds(syncCntDiff);
249 // // LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}, syncInCntCorr {}, syncOutCntCorr {}",
250 // // nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(),
251 // // timeSyncMaster->syncOutCnt, obs->timeOutputs.syncInCnt, syncCntDiff, _syncInCntCorr, _syncOutCntCorr);
252 // }
253 // else
254 // {
255 // return;
256 // }
257 // }
258
259 ✗ convertedData = obs;
260 ✗ }
261 else /* if (_outputType == OutputType_EmlidObs) */
262 {
263 ✗ auto obs = std::make_shared<EmlidObs>();
264 ✗ auto packet = uartPacket->raw;
265 ✗ vendor::emlid::decryptEmlidObs(obs, packet);
266 ✗ convertedData = obs;
267 ✗ }
268
269 ✗ if (!convertedData) { return; }
270
271 ✗ if (convertedData->insTime.empty())
272 {
273 ✗ convertedData->insTime = util::time::GetCurrentInsTime();
274 }
275
276 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_CONVERTED, convertedData);
277 ✗ }
278