INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProvider/GNSS/FileReader/NmeaFile.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 124 147 84.4%
Functions: 14 17 82.4%
Branches: 105 208 50.5%
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 "NmeaFile.hpp"
10
11 #include "util/Logger.hpp"
12 #include "Navigation/Transformations/CoordinateFrames.hpp"
13 #include "Navigation/Transformations/Units.hpp"
14 #include "util/Time/TimeBase.hpp"
15 #include "util/StringUtil.hpp"
16
17 #include "internal/FlowManager.hpp"
18 #include "NodeData/State/PosVel.hpp"
19
20 115 NAV::NmeaFile::NmeaFile()
21
3/6
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 115 times.
✗ Branch 9 not taken.
 115 : Node(typeStatic())
22 {
23 LOG_TRACE("{}: called", name);
24
25 115 _hasConfig = true;
26 115 _guiConfigDefaultWindowSize = { 517, 87 };
27
28
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateOutputPin("PosVel", Pin::Type::Flow, { NAV::PosVel::type() }, &NmeaFile::pollData);
29 230 }
30
31 232 NAV::NmeaFile::~NmeaFile()
32 {
33 LOG_TRACE("{}: called", nameId());
34 232 }
35
36 229 std::string NAV::NmeaFile::typeStatic()
37 {
38
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 458 return "NmeaFile";
39 }
40
41 ✗ std::string NAV::NmeaFile::type() const
42 {
43 ✗ return typeStatic();
44 }
45
46 114 std::string NAV::NmeaFile::category()
47 {
48
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Provider";
49 }
50
51 ✗ void NAV::NmeaFile::guiConfig()
52 {
53 ✗ if (auto res = FileReader::guiConfig(".*", { ".*" }, size_t(id), nameId()))
54 {
55 ✗ LOG_DEBUG("{}: Path changed to {}", nameId(), _path);
56 ✗ flow::ApplyChanges();
57 ✗ if (res == FileReader::PATH_CHANGED)
58 {
59 ✗ doReinitialize();
60 }
61 else
62 {
63 ✗ doDeinitialize();
64 }
65 }
66 ✗ }
67
68 ✗ [[nodiscard]] json NAV::NmeaFile::save() const
69 {
70 LOG_TRACE("{}: called", nameId());
71
72 ✗ json j;
73
74 ✗ j["FileReader"] = FileReader::save();
75
76 ✗ return j;
77 ✗ }
78
79 1 void NAV::NmeaFile::restore(json const& j)
80 {
81 LOG_TRACE("{}: called", nameId());
82
83
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("FileReader"))
84 {
85 1 FileReader::restore(j.at("FileReader"));
86 }
87 1 }
88
89 1 bool NAV::NmeaFile::initialize()
90 {
91 LOG_TRACE("{}: called", nameId());
92
93 1 return FileReader::initialize();
94 }
95
96 1 void NAV::NmeaFile::deinitialize()
97 {
98 LOG_TRACE("{}: called", nameId());
99
100 1 FileReader::deinitialize();
101 1 }
102
103 2 bool NAV::NmeaFile::resetNode()
104 {
105 2 FileReader::resetReader();
106
107 2 _hasValidDate = false;
108 2 _oldSoD = -1.0;
109
110 2 return true;
111 }
112
113 1 bool NAV::NmeaFile::setDateFromZDA(const std::string& line)
114 {
115 // decode ZDA string according to http://www.nmea.de/nmea0183datensaetze.html#zda
116
117 // 0 1 2 3 4 5 6
118 // | | | | | | |
119 // $--ZDA,hhmmss.ss,xx,xx,xxxx,xx,xx*hh<CR><LF>
120
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::vector<std::string> splittedString = str::split(line, ",");
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (splittedString.size() == 7)
122 {
123 1 std::size_t pos_star = splittedString.back().find('*');
124
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (pos_star != std::string::npos)
125 {
126
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 int64_t crc = std::strtol(splittedString[6].substr(pos_star + 1).c_str(), nullptr, 16);
127 // checksum calculation similar to https://gist.github.com/devendranaga/fce8e166f4335fa777650493cb9246db
128 1 int64_t mycrc = 0;
129 1 pos_star = line.find('*');
130
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 33 for (unsigned int i = 1; i < pos_star; i++)
131 {
132
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 mycrc ^= line.at(i);
133 }
134
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (mycrc == crc)
135 {
136
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 _currentDate.day = std::stoi(splittedString[2]);
137
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 _currentDate.month = std::stoi(splittedString[3]);
138
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 _currentDate.year = std::stoi(splittedString[4]);
139
140 1 _hasValidDate = true;
141 1 return true;
142 }
143 }
144 }
145 ✗ return false;
146 1 }
147
148 3 bool NAV::NmeaFile::setDateFromRMC(const std::string& line)
149 {
150 // decode RMC string according to http://www.nmea.de/nmea0183datensaetze.html#rmc
151
152 // 0 1 2 3 4 5 6 7 8 9 10 11
153 // | | | | | | | | | | | |
154 // $--RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a*hh<CR><LF>
155
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 std::vector<std::string> splittedString = str::split(line, ",");
156
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 if (splittedString.size() == 12)
157 {
158 3 std::size_t pos_star = splittedString[11].find('*');
159
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (pos_star != std::string::npos)
160 {
161
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 int64_t crc = std::strtol(splittedString[11].substr(pos_star + 1).c_str(), nullptr, 16);
162 // checksum calculation similar to https://gist.github.com/devendranaga/fce8e166f4335fa777650493cb9246db
163 3 int64_t mycrc = 0;
164 3 pos_star = line.find('*');
165
2/2
✓ Branch 0 taken 206 times.
✓ Branch 1 taken 3 times.
 209 for (unsigned int i = 1; i < pos_star; i++)
166 {
167
1/2
✓ Branch 1 taken 206 times.
✗ Branch 2 not taken.
 206 mycrc ^= line.at(i);
168 }
169
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (mycrc == crc)
170 {
171
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 _currentDate.day = std::stoi(splittedString[9].substr(0, 2));
172
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 _currentDate.month = std::stoi(splittedString[9].substr(2, 2));
173
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 _currentDate.year = std::stoi(splittedString[9].substr(4, 2));
174
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (_currentDate.year > 60)
175 {
176 ✗ _currentDate.year += 1900;
177 }
178 else
179 {
180 3 _currentDate.year += 2000;
181 }
182
183 3 _hasValidDate = true;
184 3 return true;
185 }
186 }
187 }
188 ✗ return false;
189 3 }
190
191 4 std::shared_ptr<const NAV::NodeData> NAV::NmeaFile::pollData()
192 {
193
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 auto obs = std::make_shared<PosVel>();
194
195 // Read line
196 4 std::string line;
197
198 4 std::vector<std::string> splittedData;
199
200 4 int hour = 0;
201 4 int minute = 0;
202 4 double second = 0.0;
203
204 4 double lat_rad = 0.0;
205 4 double lon_rad = 0.0;
206 4 double hgt = 0.0;
207
208 while (true)
209 {
210
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 getline(line);
211
212
3/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 12 times.
 13 if (eof())
213 {
214 1 return nullptr; // when done with file reading
215 }
216
217 // Remove any starting non text characters
218
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 23 line.erase(line.begin(), std::ranges::find_if(line, [](int ch) { return std::isgraph(ch); }));
219
220
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 splittedData = str::split(line, ",");
221
222
2/2
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 2 times.
 12 if (splittedData[0].starts_with("$"))
223 {
224
10/14
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 3 times.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4 times.
✓ Branch 10 taken 3 times.
✓ Branch 11 taken 7 times.
✓ Branch 12 taken 3 times.
✓ Branch 14 taken 4 times.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 10 if (_hasValidDate && splittedData[0].substr(3, 3) == "GGA")
225 {
226 // decode GGA stream according to http://www.nmea.de/nmea0183datensaetze.html#gga
227
228 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14
229 // | | | | | | | | | | | | | | |
230 // $--GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh<CR><LF>
231
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (splittedData.size() != 15) { continue; }
232
233 4 std::size_t pos_star = splittedData[14].find('*');
234
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 int64_t crc = std::strtol(splittedData[14].substr(pos_star + 1).c_str(), nullptr, 16);
235 // checksum calculation similar to https://gist.github.com/devendranaga/fce8e166f4335fa777650493cb9246db
236 4 int64_t mycrc = 0;
237 4 pos_star = line.find('*');
238
2/2
✓ Branch 0 taken 271 times.
✓ Branch 1 taken 4 times.
 275 for (unsigned int i = 1; i < pos_star; i++)
239 {
240
1/2
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
 271 mycrc ^= line.at(i);
241 }
242
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 if (mycrc == crc)
243 {
244
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 hour = std::stoi(splittedData[1].substr(0, 2));
245
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 minute = std::stoi(splittedData[1].substr(2, 2));
246
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 second = std::stod(splittedData[1].substr(4));
247 3 double newSOD = hour * 24 * 60.0 + minute * 60.0 + second;
248
249 // only continue if second of day > than previous one
250
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (newSOD < _oldSoD)
251 {
252 ✗ _oldSoD = newSOD; // store current second of day for next call of this routine
253 ✗ _hasValidDate = false; // force wait until next ZDA stream
254 ✗ continue;
255 }
256
257
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 double lat1 = std::stod(splittedData[2].substr(0, 2));
258
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 double lat2 = std::stod(splittedData[2].substr(2));
259
260 3 lat_rad = (lat1 + lat2 / 60.0) / 180.0 * M_PI; // convert to radian
261
262
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
 3 if (splittedData[3] == "S") // flip sign if south latitude
263 {
264 ✗ lat_rad *= -1.0;
265 }
266
267
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 double lon1 = std::stoi(splittedData[4].substr(0, 3));
268
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 double lon2 = std::stod(splittedData[4].substr(3));
269
270 3 lon_rad = (lon1 + lon2 / 60.0) / 180.0 * M_PI; // convert to radian
271
272
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
 3 if (splittedData[5] == "W") // flip sign if west longitude
273 {
274 ✗ lon_rad *= -1.0;
275 }
276
277
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 hgt = std::stod(splittedData[9]) + std::stod(splittedData[11]); // ellipsoidal height = height above geoid + geoid height
278
279 3 break;
280 }
281 }
282
4/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 5 times.
 6 else if (splittedData[0].substr(3, 3) == "ZDA")
283 {
284
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (setDateFromZDA(line))
285 {
286 1 _oldSoD = -1;
287 }
288 1 continue;
289 }
290
4/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 2 times.
 5 else if (splittedData[0].substr(3, 3) == "RMC")
291 {
292
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 if (setDateFromRMC(line))
293 {
294 3 _oldSoD = -1;
295 }
296 3 continue;
297 }
298 }
299 9 }
300
301
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Eigen::Vector3d lla_pos{ lat_rad, lon_rad, hgt };
302
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Eigen::Vector3d n_vel{ std::nan(""), std::nan(""), std::nan("") }; // GGA streams don't contain velocity, thus set this one invalid
303
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 obs->insTime = InsTime(_currentDate.year, _currentDate.month, _currentDate.day,
304 hour, minute, second,
305 3 UTC); // per definition, time tags in GGA NMEA streams are in UTC
306
307
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 obs->setPosition_lla(lla_pos);
308
309
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 obs->setVelocity_n(n_vel);
310
311
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 invokeCallbacks(OUTPUT_PORT_INDEX_NMEA_POS_OBS, obs);
312 3 return obs;
313 4 }
314
315 1 NAV::FileReader::FileType NAV::NmeaFile::determineFileType()
316 {
317 1 return FileReader::FileType::ASCII;
318 }
319
320 1 void NAV::NmeaFile::readHeader()
321 {
322 // Empty because NMEA does not have a header
323 1 }
324