INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Converter/GNSS/UartPacketConverter.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 13 128 10.2%
Functions: 4 10 40.0%
Branches: 12 259 4.6%
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 ✗ return true;
187 }
188
189 ✗ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
190 {
191 ✗ auto uartPacket = std::static_pointer_cast<const UartPacket>(queue.extract_front());
192
193 ✗ std::shared_ptr<NodeData> convertedData = nullptr;
194 ✗ if (_outputType == OutputType_UbloxObs)
195 {
196 ✗ auto obs = std::make_shared<UbloxObs>();
197 ✗ auto packet = uartPacket->raw;
198 ✗ if (!vendor::ublox::decryptUbloxObs(obs, packet, nameId())) { return; };
199 ✗ convertedData = obs;
200 ✗ }
201 ✗ else if (_outputType == OutputType_WiFiObs)
202 {
203 ✗ auto obs = std::make_shared<WiFiObs>();
204 ✗ auto packet = uartPacket->raw;
205
206 ✗ vendor::espressif::decryptWiFiObs(obs, packet, nameId());
207 ✗ if (_syncInPin && inputPins.at(1).isPinLinked())
208 {
209 ✗ auto timeSyncMaster = getInputValue<const NAV::VectorNavSensor::TimeSync>(0);
210 ✗ if (_lastSyncInCnt > obs->timeOutputs.syncInCnt) // reset of the slave
211 {
212 ✗ _syncOutCntCorr = 0;
213 ✗ _syncInCntCorr = timeSyncMaster->v->syncOutCnt;
214 }
215 ✗ else if (_lastSyncOutCnt > timeSyncMaster->v->syncOutCnt) // reset of the master
216 {
217 ✗ _syncInCntCorr = 0;
218 ✗ _syncOutCntCorr = obs->timeOutputs.syncInCnt;
219 }
220 ✗ else if (_lastSyncOutCnt == 0 && timeSyncMaster->v->syncOutCnt > 1) // slave counter started later
221 {
222 ✗ _syncInCntCorr = timeSyncMaster->v->syncOutCnt;
223 }
224 ✗ else if (_lastSyncOutCnt == 0 && obs->timeOutputs.syncInCnt > 1) // master counter started later
225 {
226 ✗ _syncOutCntCorr = obs->timeOutputs.syncInCnt;
227 }
228 ✗ _lastSyncOutCnt = timeSyncMaster->v->syncOutCnt;
229 ✗ _lastSyncInCnt = obs->timeOutputs.syncInCnt;
230 ✗ int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->v->syncOutCnt - _syncOutCntCorr;
231 ✗ obs->insTime = timeSyncMaster->v->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn)
232 ✗ + std::chrono::seconds(syncCntDiff);
233 // LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}, syncInCntCorr {}, syncOutCntCorr {}",
234 // nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(),
235 // timeSyncMaster->syncOutCnt, obs->timeOutputs.syncInCnt, syncCntDiff, _syncInCntCorr, _syncOutCntCorr);
236 ✗ }
237 ✗ convertedData = obs;
238 ✗ }
239 else /* if (_outputType == OutputType_EmlidObs) */
240 {
241 ✗ auto obs = std::make_shared<EmlidObs>();
242 ✗ auto packet = uartPacket->raw;
243 ✗ vendor::emlid::decryptEmlidObs(obs, packet);
244 ✗ convertedData = obs;
245 ✗ }
246
247 ✗ if (!convertedData->insTime.empty())
248 {
249 ✗ if (util::time::GetMode() == util::time::Mode::REAL_TIME)
250 {
251 ✗ util::time::SetCurrentTime(convertedData->insTime);
252 }
253 }
254 ✗ else if (auto currentTime = util::time::GetCurrentInsTime();
255 ✗ !currentTime.empty())
256 {
257 ✗ convertedData->insTime = currentTime;
258 }
259
260 ✗ if (convertedData)
261 {
262 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_CONVERTED, convertedData);
263 }
264 ✗ }
265