INSTINCT Code Coverage Report

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