INSTINCT Code Coverage Report

Directory: src/
File: util/Vendor/Ublox/UbloxUartSensor.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 93 118 78.8%
Functions: 5 8 62.5%
Branches: 83 170 48.8%
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 "UbloxUartSensor.hpp"
10
11 #include "UbloxUtilities.hpp"
12
13 #include "util/Container/STL.hpp"
14
15 226 NAV::vendor::ublox::UbloxUartSensor::UbloxUartSensor(std::string name)
16
2/4
✓ Branch 3 taken 226 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 226 times.
✗ Branch 7 not taken.
 452 : _name(std::move(name)), _buffer(uart::sensors::UartSensor::DefaultReadBufferSize)
17 {
18
1/2
✓ Branch 1 taken 226 times.
✗ Branch 2 not taken.
 226 resetTracking();
19 226 }
20
21 127650 void NAV::vendor::ublox::UbloxUartSensor::resetTracking()
22 {
23 LOG_DATA("{}: Reset tracking", _name);
24 127650 _currentlyBuildingBinaryPacket = false;
25 127650 _currentlyBuildingAsciiPacket = false;
26
27 127650 _asciiEndChar1Found = false;
28 127650 _binarySyncChar2Found = false;
29 127650 _binaryMsgClassFound = false;
30 127650 _binaryMsgIdFound = false;
31 127650 _binaryPayloadLength1Found = false;
32 127650 _binaryPayloadLength2Found = false;
33
34 #if (defined(__GNUC__) || defined(__GNUG__)) && !defined(__clang__)
35 #pragma GCC diagnostic push
36 #pragma GCC diagnostic ignored "-Wstringop-overflow"
37 #endif
38 127650 _binaryMsgClass = 0;
39 127650 _binaryMsgId = 0;
40 127650 _binaryPayloadLength = 0;
41 #if (defined(__GNUC__) || defined(__GNUG__)) && !defined(__clang__)
42 #pragma GCC diagnostic pop
43 #endif
44
45 127650 _buffer.resize(0);
46 127650 _numOfBytesRemainingForCompletePacket = 0;
47 #if LOG_LEVEL <= LOG_LEVEL_DATA
48 _unrecognizedBytes.clear();
49 #endif
50 127650 }
51
52 5305878 std::unique_ptr<uart::protocol::Packet> NAV::vendor::ublox::UbloxUartSensor::findPacket(uint8_t dataByte)
53 {
54
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 5305878 times.
 5305878 if (_buffer.size() == _buffer.capacity())
55 {
56 // Buffer is full
57 ✗ resetTracking();
58 ✗ LOG_ERROR("{}: Discarding current packet, because buffer is full", _name);
59 }
60
61
4/4
✓ Branch 0 taken 2663946 times.
✓ Branch 1 taken 2641932 times.
✓ Branch 2 taken 63712 times.
✓ Branch 3 taken 2600234 times.
 5305878 if (!_currentlyBuildingAsciiPacket && !_currentlyBuildingBinaryPacket)
62 {
63 #if LOG_LEVEL <= LOG_LEVEL_DATA
64 if (dataByte == BINARY_SYNC_CHAR_1 || dataByte == ASCII_START_CHAR)
65 {
66 if (!_unrecognizedBytes.empty())
67 {
68 LOG_DATA("{}: {} unrecognized bytes since last message: {}", _name, _unrecognizedBytes.size(), joinToString(_unrecognizedBytes, " ", ":x"));
69 }
70 }
71 else
72 {
73 _unrecognizedBytes.push_back(dataByte);
74 }
75 #endif
76 // This byte must be the start char
77
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 46854 times.
 63712 if (dataByte == BINARY_SYNC_CHAR_1)
78 {
79 LOG_DATA("{}: 1st sync character found", _name);
80 16858 resetTracking();
81 16858 _currentlyBuildingBinaryPacket = true;
82 16858 _buffer.push_back(dataByte);
83 }
84
1/2
✓ Branch 0 taken 46854 times.
✗ Branch 1 not taken.
 46854 else if (dataByte == ASCII_START_CHAR)
85 {
86 LOG_DATA("{}: Ascii sync character found", _name);
87 46854 resetTracking();
88 46854 _currentlyBuildingAsciiPacket = true;
89 46854 _buffer.push_back(dataByte);
90 }
91 }
92
2/2
✓ Branch 0 taken 2600234 times.
✓ Branch 1 taken 2641932 times.
 5242166 else if (_currentlyBuildingBinaryPacket)
93 {
94 2600234 _buffer.push_back(dataByte);
95
96
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 2583376 times.
 2600234 if (!_binarySyncChar2Found)
97 {
98 // This byte must be the second sync char
99
1/2
✓ Branch 0 taken 16858 times.
✗ Branch 1 not taken.
 16858 if (dataByte == BINARY_SYNC_CHAR_2)
100 {
101 16858 _binarySyncChar2Found = true;
102 LOG_DATA("{}: 2nd sync character found", _name);
103 }
104 else
105 {
106 ✗ resetTracking();
107 }
108 }
109
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 2566518 times.
 2583376 else if (!_binaryMsgClassFound)
110 {
111 // This byte must be the message class
112 16858 _binaryMsgClassFound = true;
113 16858 _binaryMsgClass = dataByte;
114 }
115
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 2549660 times.
 2566518 else if (!_binaryMsgIdFound)
116 {
117 // This byte must be the message id
118 16858 _binaryMsgIdFound = true;
119 16858 _binaryMsgId = dataByte;
120 }
121
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 2532802 times.
 2549660 else if (!_binaryPayloadLength1Found)
122 {
123 16858 _binaryPayloadLength1Found = true;
124 16858 _binaryPayloadLength = static_cast<uint16_t>(dataByte);
125 }
126
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 2515944 times.
 2532802 else if (!_binaryPayloadLength2Found)
127 {
128 16858 _binaryPayloadLength2Found = true;
129 16858 _binaryPayloadLength |= static_cast<uint16_t>(static_cast<uint16_t>(dataByte) << 8U);
130 16858 _binaryPayloadLength = uart::stoh(_binaryPayloadLength, ENDIANNESS);
131 16858 _numOfBytesRemainingForCompletePacket = _binaryPayloadLength + 2U;
132
133 LOG_DATA("{}: Binary packet: Class={:0x} [{}], Id={:0x} [{}], payload length={}", _name,
134 _binaryMsgClass, getStringFromMsgClass(static_cast<UbxClass>(_binaryMsgClass)),
135 _binaryMsgId, getStringFromMsgId(static_cast<UbxClass>(_binaryMsgClass), _binaryMsgId), _binaryPayloadLength);
136 }
137 else
138 {
139 // We are currently collecting data for our packet.
140 2515944 _numOfBytesRemainingForCompletePacket--;
141
142
2/2
✓ Branch 0 taken 16858 times.
✓ Branch 1 taken 2499086 times.
 2515944 if (_numOfBytesRemainingForCompletePacket == 0)
143 {
144 // We have a possible binary packet!
145
1/2
✓ Branch 1 taken 16858 times.
✗ Branch 2 not taken.
 16858 auto p = std::make_unique<uart::protocol::Packet>(_buffer, &_sensor);
146
147
3/4
✓ Branch 2 taken 16858 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16844 times.
✓ Branch 5 taken 14 times.
 16858 if (p->isValid())
148 {
149 // We have a valid binary packet!!!.
150
1/2
✓ Branch 1 taken 16844 times.
✗ Branch 2 not taken.
 16844 resetTracking();
151 16844 return p;
152 }
153 // Invalid packet!
154
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 LOG_DEBUG("{}: Invalid binary packet: Class={:0x}, Id={:0x}, payload length={}", _name, _binaryMsgClass, _binaryMsgId, _binaryPayloadLength);
155
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 resetTracking();
156 16858 }
157 }
158 }
159
1/2
✓ Branch 0 taken 2641932 times.
✗ Branch 1 not taken.
 2641932 else if (_currentlyBuildingAsciiPacket)
160 {
161 2641932 _buffer.push_back(dataByte);
162
163
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2641932 times.
 2641932 if (dataByte == ASCII_ESCAPE_CHAR)
164 {
165 ✗ resetTracking();
166 }
167
2/2
✓ Branch 0 taken 46854 times.
✓ Branch 1 taken 2595078 times.
 2641932 else if (dataByte == ASCII_END_CHAR_1)
168 {
169 46854 _asciiEndChar1Found = true;
170 }
171
2/2
✓ Branch 0 taken 46854 times.
✓ Branch 1 taken 2548224 times.
 2595078 else if (_asciiEndChar1Found)
172 {
173
1/2
✓ Branch 0 taken 46854 times.
✗ Branch 1 not taken.
 46854 if (dataByte == ASCII_END_CHAR_2)
174 {
175 // We have a possible data packet
176
1/2
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
 46854 auto p = std::make_unique<uart::protocol::Packet>(_buffer, &_sensor);
177
178
2/4
✓ Branch 2 taken 46854 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 46854 times.
✗ Branch 5 not taken.
 46854 if (p->isValid())
179 {
180 // We have a valid ascii packet!!!.
181 LOG_DATA("{}: Valid ascii packet: {}", _name, p->datastr().substr(0, p->getRawDataLength() - 2));
182
1/2
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
 46854 resetTracking();
183 46854 return p;
184 }
185 // Invalid packet!
186 ✗ LOG_ERROR("{}: Invalid ascii packet: {}", _name, p->datastr());
187 46854 }
188 else
189 {
190 LOG_DATA("{}: 2nd Ascii end character not found", _name);
191 }
192 ✗ resetTracking();
193 }
194 }
195
196 5242180 return nullptr;
197 }
198
199 ✗ void NAV::vendor::ublox::UbloxUartSensor::packetFinderFunction(const std::vector<uint8_t>& data, const uart::xplat::TimeStamp& timestamp, uart::sensors::UartSensor::ValidPacketFoundHandler dispatchPacket, void* dispatchPacketUserData, void* userData)
200 {
201 ✗ auto* sensor = static_cast<UbloxUartSensor*>(userData);
202
203 ✗ for (size_t i = 0; i < data.size(); i++, sensor->_runningDataIndex++)
204 {
205 ✗ auto packetPointer = sensor->findPacket(data.at(i));
206
207 ✗ if (packetPointer != nullptr)
208 {
209 ✗ uart::protocol::Packet packet = *packetPointer;
210 ✗ dispatchPacket(dispatchPacketUserData, packet, sensor->_runningDataIndex, timestamp);
211 ✗ }
212 ✗ }
213 ✗ }
214
215 254598 uart::protocol::Packet::Type NAV::vendor::ublox::UbloxUartSensor::packetTypeFunction(const uart::protocol::Packet& packet)
216 {
217
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 254598 times.
 254598 if (packet.getRawDataLength() < 1)
218 {
219 ✗ LOG_CRITICAL("Packet does not contain any data.");
220 }
221
222
2/2
✓ Branch 2 taken 187416 times.
✓ Branch 3 taken 67182 times.
 254598 if (packet.getRawData().at(0) == '$')
223 {
224 187416 return uart::protocol::Packet::Type::TYPE_ASCII;
225 }
226
1/2
✓ Branch 2 taken 67182 times.
✗ Branch 3 not taken.
 67182 if (packet.getRawData().at(0) == BINARY_SYNC_CHAR_1)
227 {
228
1/2
✓ Branch 2 taken 67182 times.
✗ Branch 3 not taken.
 67182 if (packet.getRawData().at(1) == BINARY_SYNC_CHAR_2)
229 {
230 67182 return uart::protocol::Packet::Type::TYPE_BINARY;
231 }
232 }
233
234 ✗ return uart::protocol::Packet::Type::TYPE_UNKNOWN;
235 }
236
237 63712 bool NAV::vendor::ublox::UbloxUartSensor::checksumFunction(const uart::protocol::Packet& packet)
238 {
239
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 63698 times.
 63712 if (packet.getRawDataLength() <= 8)
240 {
241 14 return false;
242 }
243
244
2/2
✓ Branch 1 taken 46854 times.
✓ Branch 2 taken 16844 times.
 63698 if (packet.type() == uart::protocol::Packet::Type::TYPE_ASCII)
245 {
246 // First check if we have a checksum at all
247
4/8
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46854 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46854 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 46854 times.
 46854 if (packet.getRawData().at(packet.getRawDataLength() - 5) != '*')
248 {
249 ✗ return false;
250 }
251
252 // Return true, if a wildcard checksum is present
253
3/6
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46854 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46854 times.
✗ Branch 8 not taken.
 46854 if (packet.getRawData().at(packet.getRawDataLength() - 3) == 'X'
254
2/12
✗ Branch 0 not taken.
✓ Branch 1 taken 46854 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 46854 times.
 46854 && packet.getRawData().at(packet.getRawDataLength() - 4) == 'X')
255 {
256 ✗ return true;
257 }
258
259
2/4
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46854 times.
✗ Branch 5 not taken.
 46854 uint8_t checksumHex = ublox::checksumNMEA(packet.getRawData());
260
3/6
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46854 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46854 times.
✗ Branch 8 not taken.
 46854 std::array<uint8_t, 2> checksumRecv = { packet.getRawData().at(packet.getRawDataLength() - 4),
261
3/6
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46854 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46854 times.
✗ Branch 8 not taken.
 46854 packet.getRawData().at(packet.getRawDataLength() - 3) };
262
1/2
✓ Branch 1 taken 46854 times.
✗ Branch 2 not taken.
 46854 return uart::to_uint8_from_hexstr(reinterpret_cast<char*>(checksumRecv.data())) == checksumHex;
263 }
264
265
1/2
✓ Branch 1 taken 16844 times.
✗ Branch 2 not taken.
 16844 if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY)
266 {
267
2/4
✓ Branch 1 taken 16844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16844 times.
✗ Branch 5 not taken.
 16844 std::pair<uint8_t, uint8_t> checksum = ublox::checksumUBX(packet.getRawData());
268
269
3/6
✓ Branch 1 taken 16844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16844 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16844 times.
✗ Branch 8 not taken.
 16844 return packet.getRawData().at(packet.getRawDataLength() - 2) == checksum.first
270
5/10
✓ Branch 0 taken 16844 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 16844 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16844 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16844 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 16844 times.
✗ Branch 12 not taken.
 16844 && packet.getRawData().at(packet.getRawDataLength() - 1) == checksum.second;
271 }
272
273 ✗ LOG_CRITICAL("Can't calculate checksum of packet with unknown type");
274 return false;
275 }
276
277 ✗ bool NAV::vendor::ublox::UbloxUartSensor::isErrorFunction([[maybe_unused]] const uart::protocol::Packet& packet)
278 {
279 ✗ return false;
280 }
281
282 ✗ bool NAV::vendor::ublox::UbloxUartSensor::isResponseFunction([[maybe_unused]] const uart::protocol::Packet& packet)
283 {
284 ✗ return false;
285 }
286