INSTINCT Code Coverage Report

Directory: src/
File: util/Vendor/Ublox/UbloxUtilities.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 90 172 52.3%
Functions: 3 3 100.0%
Branches: 79 172 45.9%
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 "UbloxUtilities.hpp"
10 #include "UbloxTypes.hpp"
11
12 #include "util/Eigen.hpp"
13 #include "Navigation/Transformations/CoordinateFrames.hpp"
14 #include "util/Logger.hpp"
15
16 #include "util/Time/TimeBase.hpp"
17
18 63701 bool NAV::vendor::ublox::decryptUbloxObs(const std::shared_ptr<NAV::UbloxObs>& obs, uart::protocol::Packet& packet, [[maybe_unused]] const std::string& nameId)
19 {
20
2/2
✓ Branch 1 taken 16847 times.
✓ Branch 2 taken 46854 times.
 63701 if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY)
21 {
22 16847 uint8_t msgClassByte = packet.extractUint8();
23 16847 obs->msgClass = static_cast<UbxClass>(msgClassByte);
24 16847 obs->msgId = packet.extractUint8();
25 16847 obs->payloadLength = packet.extractUint16();
26
27 // Ack/Nak Messages: Acknowledge or Reject messages to UBX-CFG input messages
28
2/2
✓ Branch 1 taken 46 times.
✓ Branch 2 taken 16801 times.
 16847 if (obs->msgClass == UbxClass::UBX_CLASS_ACK)
29 {
30 46 auto msgId = static_cast<UbxAckMessages>(obs->msgId);
31
1/2
✓ Branch 0 taken 46 times.
✗ Branch 1 not taken.
 46 if (msgId == UbxAckMessages::UBX_ACK_ACK)
32 {
33 46 obs->data = UbxAckAck();
34 46 std::get<UbxAckAck>(obs->data).clsID = packet.extractUint8();
35 46 std::get<UbxAckAck>(obs->data).msgID = packet.extractUint8();
36
37 LOG_DATA("{}: UBX: ACK-ACK, clsID {}, msgID {}", nameId, std::get<UbxAckAck>(obs->data).clsID, std::get<UbxAckAck>(obs->data).msgID);
38 }
39 ✗ else if (msgId == UbxAckMessages::UBX_ACK_NAK)
40 {
41 ✗ obs->data = UbxAckNak();
42 ✗ std::get<UbxAckNak>(obs->data).clsID = packet.extractUint8();
43 ✗ std::get<UbxAckNak>(obs->data).msgID = packet.extractUint8();
44
45 LOG_DATA("{}: UBX: ACK-NAK, Size {}", nameId, packet.getRawDataLength());
46 }
47 else
48 {
49 LOG_DATA("{}: UBX: ACK-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
50 ✗ return false;
51 }
52 }
53 // Configuration Input Messages: Configure the receiver
54
2/2
✓ Branch 1 taken 93 times.
✓ Branch 2 taken 16708 times.
 16801 else if (obs->msgClass == UbxClass::UBX_CLASS_CFG)
55 {
56 93 [[maybe_unused]] auto msgId = static_cast<UbxCfgMessages>(obs->msgId);
57 LOG_DATA("{}: UBX: CFG-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
58 93 return false;
59 }
60 // External Sensor Fusion Messages: External Sensor Measurements and Status Information
61
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16708 times.
 16708 else if (obs->msgClass == UbxClass::UBX_CLASS_ESF)
62 {
63 ✗ auto msgId = static_cast<UbxEsfMessages>(obs->msgId);
64 ✗ if (msgId == UbxEsfMessages::UBX_ESF_INS)
65 {
66 LOG_DATA("{}: UBX: ESF-INS, Size {}, not implemented yet!", nameId, packet.getRawDataLength());
67 ✗ return false;
68 }
69 ✗ if (msgId == UbxEsfMessages::UBX_ESF_MEAS)
70 {
71 LOG_DATA("{}: UBX: ESF-MEAS, Size {}, not implemented yet!", nameId, packet.getRawDataLength());
72 ✗ return false;
73 }
74 ✗ if (msgId == UbxEsfMessages::UBX_ESF_RAW)
75 {
76 ✗ obs->data = UbxEsfRaw();
77 ✗ for (auto& reserved1 : std::get<UbxEsfRaw>(obs->data).reserved1)
78 {
79 ✗ reserved1 = packet.extractUint8();
80 }
81 ✗ for (int i = 0; i < (obs->payloadLength - 4) / 8; i++)
82 {
83 ✗ NAV::vendor::ublox::UbxEsfRaw::UbxEsfRawData ubxEsfRawData;
84 ✗ ubxEsfRawData.data = packet.extractUint32();
85 ✗ ubxEsfRawData.sTtag = packet.extractUint32();
86 ✗ std::get<UbxEsfRaw>(obs->data).data.push_back(ubxEsfRawData);
87 }
88
89 // TODO: - UBX_ESF_RAW: Calculate the insTime somehow from the sensor time tag (sTtag)
90
91 LOG_DATA("{}: UBX: ESF-RAW, {} measurements", nameId, (obs->payloadLength - 4) / 8);
92 }
93 ✗ else if (msgId == UbxEsfMessages::UBX_ESF_STATUS)
94 {
95 LOG_DATA("{}: UBX: ESF-STATUS, Size {}, not implemented yet!", nameId, packet.getRawDataLength());
96 ✗ return false;
97 }
98 else
99 {
100 LOG_DATA("{}: UBX: ESF-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
101 ✗ return false;
102 }
103 }
104 // High Rate Navigation Results Messages: High rate time, position, speed, heading
105
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16708 times.
 16708 else if (obs->msgClass == UbxClass::UBX_CLASS_HNR)
106 {
107 ✗ [[maybe_unused]] auto msgId = static_cast<UbxHnrMessages>(obs->msgId);
108 LOG_DATA("{}: UBX: HNR-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
109 ✗ return false;
110 }
111 // Information Messages: Printf-Style Messages, with IDs such as Error, Warning, Notice
112
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16708 times.
 16708 else if (obs->msgClass == UbxClass::UBX_CLASS_INF)
113 {
114 ✗ [[maybe_unused]] auto msgId = static_cast<UbxInfMessages>(obs->msgId);
115 LOG_DATA("{}: UBX: INF-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
116 ✗ return false;
117 }
118 // Logging Messages: Log creation, deletion, info and retrieval
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16708 times.
 16708 else if (obs->msgClass == UbxClass::UBX_CLASS_LOG)
120 {
121 ✗ [[maybe_unused]] auto msgId = static_cast<UbxLogMessages>(obs->msgId);
122 LOG_DATA("{}: UBX: LOG-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
123 ✗ return false;
124 }
125 // Multiple GNSS Assistance Messages: Assistance data for various GNSS
126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16708 times.
 16708 else if (obs->msgClass == UbxClass::UBX_CLASS_MGA)
127 {
128 ✗ [[maybe_unused]] auto msgId = static_cast<UbxMgaMessages>(obs->msgId);
129 LOG_DATA("{}: UBX: MGA-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
130 ✗ return false;
131 }
132 // Monitoring Messages: Communication Status, CPU Load, Stack Usage, Task Status
133
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 16694 times.
 16708 else if (obs->msgClass == UbxClass::UBX_CLASS_MON)
134 {
135 14 [[maybe_unused]] auto msgId = static_cast<UbxMonMessages>(obs->msgId);
136 LOG_DATA("{}: UBX: MON-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
137 14 return false;
138 }
139 // Navigation Results Messages: Position, Speed, Time, Acceleration, Heading, DOP, SVs used
140
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16694 times.
 16694 else if (obs->msgClass == UbxClass::UBX_CLASS_NAV)
141 {
142 ✗ auto msgId = static_cast<UbxNavMessages>(obs->msgId);
143 ✗ if (msgId == UbxNavMessages::UBX_NAV_ATT)
144 {
145 ✗ obs->data = UbxNavAtt();
146
147 ✗ std::get<UbxNavAtt>(obs->data).iTOW = packet.extractUint32();
148 ✗ std::get<UbxNavAtt>(obs->data).version = packet.extractUint8();
149 ✗ for (auto& reserved1 : std::get<UbxNavAtt>(obs->data).reserved1)
150 {
151 ✗ reserved1 = packet.extractUint8();
152 }
153 ✗ std::get<UbxNavAtt>(obs->data).roll = packet.extractInt32();
154 ✗ std::get<UbxNavAtt>(obs->data).pitch = packet.extractInt32();
155 ✗ std::get<UbxNavAtt>(obs->data).heading = packet.extractInt32();
156 ✗ std::get<UbxNavAtt>(obs->data).accRoll = packet.extractUint32();
157 ✗ std::get<UbxNavAtt>(obs->data).accPitch = packet.extractUint32();
158 ✗ std::get<UbxNavAtt>(obs->data).accHeading = packet.extractUint32();
159
160 // TODO: Calculate the insTime with the iTOW
161 // auto currentTime = util::time::GetCurrentInsTime();
162 // if (!currentTime.empty())
163 // {
164 // auto gpst = currentTime.toGPSweekTow();
165 // currentTime = InsTime(gpst.gpsCycle,
166 // gpst.gpsWeek,
167 // static_cast<long double>(std::get<UbxNavAtt>(obs->data).iTOW) / 1000.0L);
168 // obs->insTime = currentTime;
169 // }
170
171 LOG_DATA("{}: UBX: NAV-ATT, iTOW {}, roll {}, pitch {}, heading {}", nameId, std::get<UbxNavAtt>(obs->data).iTOW, std::get<UbxNavAtt>(obs->data).roll, std::get<UbxNavAtt>(obs->data).pitch, std::get<UbxNavAtt>(obs->data).heading);
172 }
173 ✗ else if (msgId == UbxNavMessages::UBX_NAV_POSECEF)
174 {
175 ✗ obs->data = UbxNavPosecef();
176
177 ✗ std::get<UbxNavPosecef>(obs->data).iTOW = packet.extractUint32();
178 ✗ std::get<UbxNavPosecef>(obs->data).ecefX = packet.extractInt32();
179 ✗ std::get<UbxNavPosecef>(obs->data).ecefY = packet.extractInt32();
180 ✗ std::get<UbxNavPosecef>(obs->data).ecefZ = packet.extractInt32();
181 ✗ std::get<UbxNavPosecef>(obs->data).pAcc = packet.extractUint32();
182
183 // TODO: Calculate the insTime with the iTOW
184 // auto currentTime = util::time::GetCurrentInsTime();
185 // if (!currentTime.empty())
186 // {
187 // auto gpst = currentTime.toGPSweekTow();
188 // currentTime = InsTime(gpst.gpsCycle,
189 // gpst.gpsWeek,
190 // static_cast<long double>(std::get<UbxNavPosecef>(obs->data).iTOW) / 1000.0L);
191 // obs->insTime = currentTime;
192 // }
193
194 LOG_DATA("{}: UBX: NAV-POSECEF, iTOW {}, x {}, y {}, z {}", nameId, std::get<UbxNavPosecef>(obs->data).iTOW, std::get<UbxNavPosecef>(obs->data).ecefX, std::get<UbxNavPosecef>(obs->data).ecefY, std::get<UbxNavPosecef>(obs->data).ecefZ);
195 }
196 ✗ else if (msgId == UbxNavMessages::UBX_NAV_POSLLH)
197 {
198 ✗ obs->data = UbxNavPosllh();
199
200 ✗ std::get<UbxNavPosllh>(obs->data).iTOW = packet.extractUint32();
201 ✗ std::get<UbxNavPosllh>(obs->data).lon = packet.extractInt32();
202 ✗ std::get<UbxNavPosllh>(obs->data).lat = packet.extractInt32();
203 ✗ std::get<UbxNavPosllh>(obs->data).height = packet.extractInt32();
204 ✗ std::get<UbxNavPosllh>(obs->data).hMSL = packet.extractInt32();
205 ✗ std::get<UbxNavPosllh>(obs->data).hAcc = packet.extractUint32();
206 ✗ std::get<UbxNavPosllh>(obs->data).vAcc = packet.extractUint32();
207
208 // TODO: Calculate the insTime with the iTOW
209 // auto currentTime = util::time::GetCurrentInsTime();
210 // if (!currentTime.empty())
211 // {
212 // auto gpst = currentTime.toGPSweekTow();
213 // currentTime = InsTime(gpst.gpsCycle,
214 // gpst.gpsWeek,
215 // static_cast<long double>(std::get<UbxNavPosllh>(obs->data).iTOW) / 1000.0L);
216 // obs->insTime = currentTime;
217 // }
218
219 LOG_DATA("{}: UBX: NAV-POSLLH, iTOW {}, lon {}, lat {}, height {}", nameId, std::get<UbxNavPosllh>(obs->data).iTOW, std::get<UbxNavPosllh>(obs->data).lon, std::get<UbxNavPosllh>(obs->data).lat, std::get<UbxNavPosllh>(obs->data).height);
220 }
221 ✗ else if (msgId == UbxNavMessages::UBX_NAV_VELNED)
222 {
223 ✗ obs->data = UbxNavVelned();
224
225 ✗ std::get<UbxNavVelned>(obs->data).iTOW = packet.extractUint32();
226 ✗ std::get<UbxNavVelned>(obs->data).velN = packet.extractInt32();
227 ✗ std::get<UbxNavVelned>(obs->data).velE = packet.extractInt32();
228 ✗ std::get<UbxNavVelned>(obs->data).velD = packet.extractInt32();
229 ✗ std::get<UbxNavVelned>(obs->data).speed = packet.extractUint32();
230 ✗ std::get<UbxNavVelned>(obs->data).gSpeed = packet.extractUint32();
231 ✗ std::get<UbxNavVelned>(obs->data).heading = packet.extractInt32();
232 ✗ std::get<UbxNavVelned>(obs->data).sAcc = packet.extractUint32();
233 ✗ std::get<UbxNavVelned>(obs->data).cAcc = packet.extractUint32();
234
235 // TODO: Calculate the insTime with the iTOW
236 // auto currentTime = util::time::GetCurrentInsTime();
237 // if (!currentTime.empty())
238 // {
239 // auto gpst = currentTime.toGPSweekTow();
240 // currentTime = InsTime(gpst.gpsCycle,
241 // gpst.gpsWeek,
242 // static_cast<long double>(std::get<UbxNavVelned>(obs->data).iTOW) / 1000.0L);
243 // obs->insTime = currentTime;
244 // }
245
246 LOG_DATA("{}: UBX: NAV-VELNED, iTOW {}, gSpeed {} [cm/s], heading {} [deg*1e-5], velD {} [cm/s]", nameId, std::get<UbxNavVelned>(obs->data).iTOW, std::get<UbxNavVelned>(obs->data).gSpeed, std::get<UbxNavVelned>(obs->data).heading, std::get<UbxNavVelned>(obs->data).velD);
247 }
248 else
249 {
250 LOG_DATA("{}: UBX: NAV-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
251 ✗ return false;
252 }
253 }
254 // Receiver Manager Messages: Satellite Status, RTC Status
255
1/2
✓ Branch 1 taken 16694 times.
✗ Branch 2 not taken.
 16694 else if (obs->msgClass == UbxClass::UBX_CLASS_RXM)
256 {
257 16694 auto msgId = static_cast<UbxRxmMessages>(obs->msgId);
258
2/2
✓ Branch 0 taken 3289 times.
✓ Branch 1 taken 13405 times.
 16694 if (msgId == UbxRxmMessages::UBX_RXM_RAWX)
259 {
260 3289 obs->data = UbxRxmRawx();
261
262
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3289 times.
✗ Branch 6 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).rcvTow = packet.extractDouble();
263
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3289 times.
✗ Branch 6 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).week = packet.extractUint16();
264
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3289 times.
✗ Branch 6 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).leapS = packet.extractInt8();
265
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3289 times.
✗ Branch 6 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).numMeas = packet.extractUint8();
266
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3289 times.
✗ Branch 7 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).recStat = packet.extractUint8();
267
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3289 times.
✗ Branch 6 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).version = packet.extractUint8();
268
3/4
✓ Branch 2 taken 3289 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6578 times.
✓ Branch 5 taken 3289 times.
 9867 for (auto& reserved1 : std::get<UbxRxmRawx>(obs->data).reserved1)
269 {
270
1/2
✓ Branch 1 taken 6578 times.
✗ Branch 2 not taken.
 6578 reserved1 = packet.extractUint8();
271 }
272
3/4
✓ Branch 2 taken 60655 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 57366 times.
✓ Branch 5 taken 3289 times.
 60655 for (size_t i = 0; i < std::get<UbxRxmRawx>(obs->data).numMeas; i++)
273 {
274 57366 NAV::vendor::ublox::UbxRxmRawx::UbxRxmRawxData ubxRxmRawxData;
275
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.prMes = packet.extractDouble();
276
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.cpMes = packet.extractDouble();
277
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.doMes = packet.extractFloat();
278
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.gnssId = packet.extractUint8();
279
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.svId = packet.extractUint8();
280
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.sigId = packet.extractUint8();
281
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.freqId = packet.extractUint8();
282
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.locktime = packet.extractUint16();
283
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.cno = packet.extractUint8();
284
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.prStdev = packet.extractUint8();
285
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.cpStdev = packet.extractUint8();
286
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.doStdev = packet.extractUint8();
287
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.trkStat = packet.extractUint8();
288
1/2
✓ Branch 1 taken 57366 times.
✗ Branch 2 not taken.
 57366 ubxRxmRawxData.reserved2 = packet.extractUint8();
289
2/4
✓ Branch 2 taken 57366 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57366 times.
✗ Branch 6 not taken.
 57366 std::get<UbxRxmRawx>(obs->data).data.push_back(ubxRxmRawxData);
290 }
291
292 3289 obs->insTime = InsTime(0,
293
2/4
✓ Branch 1 taken 3289 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3289 times.
✗ Branch 5 not taken.
 3289 std::get<UbxRxmRawx>(obs->data).week,
294
1/2
✓ Branch 3 taken 3289 times.
✗ Branch 4 not taken.
 3289 static_cast<long double>(std::get<UbxRxmRawx>(obs->data).rcvTow));
295
296 3289 std::string satInfo;
297
3/4
✓ Branch 2 taken 3289 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 57366 times.
✓ Branch 9 taken 3289 times.
 60655 for (const auto& sat : std::get<UbxRxmRawx>(obs->data).data)
298 {
299
2/4
✓ Branch 2 taken 57366 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57366 times.
✗ Branch 6 not taken.
 57366 satInfo += "[" + std::to_string(sat.gnssId);
300
2/4
✓ Branch 2 taken 57366 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57366 times.
✗ Branch 6 not taken.
 57366 satInfo += ", " + std::to_string(sat.svId);
301
3/6
✓ Branch 2 taken 57366 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57366 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 57366 times.
✗ Branch 9 not taken.
 57366 satInfo += ", " + std::to_string(sat.sigId) + "], ";
302 }
303
304 LOG_DATA("{}: UBX: RXM-RAWX, Size {}, rcvWeek {}, rcvTow {}, numMeas {}, satInfo {}.{}", nameId,
305 packet.getRawDataLength(), std::get<UbxRxmRawx>(obs->data).week, std::get<UbxRxmRawx>(obs->data).rcvTow,
306 std::get<UbxRxmRawx>(obs->data).numMeas, satInfo, std::get<UbxRxmRawx>(obs->data).week == 0 ? " Skipped because no valid time." : "");
307
3/4
✓ Branch 2 taken 3289 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 88 times.
✓ Branch 5 taken 3201 times.
 3289 if (std::get<UbxRxmRawx>(obs->data).week == 0)
308 {
309 88 return false;
310 }
311 3289 }
312
1/2
✓ Branch 0 taken 13405 times.
✗ Branch 1 not taken.
 13405 else if (msgId == UbxRxmMessages::UBX_RXM_SFRBX)
313 {
314 13405 obs->data = UbxRxmSfrbx();
315
316 13405 std::get<UbxRxmSfrbx>(obs->data).gnssId = packet.extractUint8();
317 13405 std::get<UbxRxmSfrbx>(obs->data).svId = packet.extractUint8();
318 13405 std::get<UbxRxmSfrbx>(obs->data).sigId = packet.extractUint8();
319 13405 std::get<UbxRxmSfrbx>(obs->data).freqId = packet.extractUint8();
320 13405 std::get<UbxRxmSfrbx>(obs->data).numWords = packet.extractUint8();
321 13405 std::get<UbxRxmSfrbx>(obs->data).chn = packet.extractUint8();
322 13405 std::get<UbxRxmSfrbx>(obs->data).version = packet.extractUint8();
323 13405 std::get<UbxRxmSfrbx>(obs->data).reserved0 = packet.extractUint8();
324
325
2/2
✓ Branch 2 taken 116920 times.
✓ Branch 3 taken 13405 times.
 130325 for (size_t i = 0; i < std::get<UbxRxmSfrbx>(obs->data).numWords; i++)
326 {
327
2/4
✓ Branch 3 taken 116920 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 116920 times.
✗ Branch 7 not taken.
 116920 std::get<UbxRxmSfrbx>(obs->data).dwrd.emplace_back(packet.extractUint32());
328 }
329
330 LOG_DATA("{}: UBX: RXM-SFRBX, gnssId {}, svId {}, freqId {}, numWords {}, chn {}, version {}", nameId,
331 std::get<UbxRxmSfrbx>(obs->data).gnssId,
332 std::get<UbxRxmSfrbx>(obs->data).svId,
333 std::get<UbxRxmSfrbx>(obs->data).freqId,
334 std::get<UbxRxmSfrbx>(obs->data).numWords,
335 std::get<UbxRxmSfrbx>(obs->data).chn,
336 std::get<UbxRxmSfrbx>(obs->data).version);
337 }
338 else
339 {
340 LOG_DATA("{}: UBX: RXM-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
341 ✗ return false;
342 }
343 }
344 // Security Feature Messages
345 ✗ else if (obs->msgClass == UbxClass::UBX_CLASS_SEC)
346 {
347 ✗ [[maybe_unused]] auto msgId = static_cast<UbxSecMessages>(obs->msgId);
348 LOG_DATA("{}: UBX: SEC-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
349 ✗ return false;
350 }
351 // Timing Messages: Time Pulse Output, Time Mark Results
352 ✗ else if (obs->msgClass == UbxClass::UBX_CLASS_TIM)
353 {
354 ✗ [[maybe_unused]] auto msgId = static_cast<UbxTimMessages>(obs->msgId);
355 LOG_DATA("{}: UBX: TIM-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
356 ✗ return false;
357 }
358 // Firmware Update Messages: Memory/Flash erase/write, Reboot, Flash identification, etc.
359 ✗ else if (obs->msgClass == UbxClass::UBX_CLASS_UPD)
360 {
361 ✗ [[maybe_unused]] auto msgId = static_cast<UbxUpdMessages>(obs->msgId);
362 LOG_DATA("{}: UBX: UPD-{:x}, Size {}, not implemented yet!", nameId, obs->msgId, packet.getRawDataLength());
363 ✗ return false;
364 }
365 else
366 {
367 LOG_DATA("{}: UBX: {:x}-{:x}, Size {}, not implemented yet!", nameId, static_cast<int>(obs->msgClass), static_cast<int>(obs->msgId), packet.getRawDataLength());
368 ✗ return false;
369 }
370 }
371 46854 else if (packet.type() == uart::protocol::Packet::Type::TYPE_ASCII)
372 {
373 LOG_DATA("{}: NMEA: {}", nameId, packet.datastr().substr(0, packet.datastr().size() - 2));
374 }
375
376 63506 return true;
377 }
378
379 16847 std::pair<uint8_t, uint8_t> NAV::vendor::ublox::checksumUBX(const std::vector<uint8_t>& data)
380 {
381 16847 uint8_t cka = 0;
382 16847 uint8_t ckb = 0;
383
384
2/2
✓ Branch 1 taken 2550572 times.
✓ Branch 2 taken 16847 times.
 2567419 for (size_t i = 2; i < data.size() - 2; i++)
385 {
386
1/2
✓ Branch 1 taken 2550572 times.
✗ Branch 2 not taken.
 2550572 cka += data.at(i);
387 2550572 ckb += cka;
388 }
389 33694 return std::make_pair(cka, ckb);
390 }
391
392 46861 uint8_t NAV::vendor::ublox::checksumNMEA(const std::vector<uint8_t>& data)
393 {
394 46861 uint8_t calcChecksum = 0;
395 // |-> <-| Checksum without first $ sign and till the *
396 // $--ZDA,hhmmss.ss,xx,xx,xxxx,xx,xx*hh<CR><LF>
397
2/2
✓ Branch 1 taken 2407924 times.
✓ Branch 2 taken 46861 times.
 2454785 for (size_t i = 1; i < data.size() - 5; i++)
398 {
399 2407924 calcChecksum ^= data.at(i);
400 }
401 46861 return calcChecksum;
402 }
403