Debian Codegraph @

libs/hwdrivers/src/CGPSInterface_parser_NMEA.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/hwdrivers/CGPSInterface.h>
13 #include <mrpt/system/filesystem.h>
14 #include <mrpt/system/os.h>
15
16 #include <iostream>
17
18 using namespace mrpt::hwdrivers;
19 using namespace mrpt::obs;
20 using namespace mrpt::system;
21 using namespace std;
22
23 const size_t MAX_NMEA_LINE_LENGTH = 1024;
24
25 bool CGPSInterface::implement_parser_NMEA(size_t& out_minimum_rx_buf_to_decide)
26 {
27 out_minimum_rx_buf_to_decide = 3;
28
29 if (m_rx_buffer.size() < 3) return true; // no need to skip a byte
30
31 const size_t nBytesAval = m_rx_buffer.size(); // Available for read
32
33 // If the string does not start with "$GP" it is not valid:
34 uint8_t buf[3]; // peek_buffer
35 m_rx_buffer.peek_many(&buf[0], 3);
36
37 // Known "talkers": Baidu, Galileo, GPS, GLONASS, etc.
38 // Ref: https://gpsd.gitlab.io/gpsd/NMEA.html
39 static const std::array<const char*, 12> known_prefixes = {"BD", "CD", "EC", "GA", "GB", "GL",
40 "GN", "GP", "II", "IN", "LC", "QZ"};
41
42 bool recognized = false;
43 if (buf[0] == '$')
44 {
45 for (const char* prefix : known_prefixes)
46 {
47 if (buf[1] == prefix[0] && buf[2] == prefix[1])
48 {
49 recognized = true;
50 break;
51 }
52 }
53 }
54 // Not the start of a NMEA string, skip 1 char:
55 if (!recognized) return false;
56
57 // It starts OK: try to find the end of the line
58 std::string line;
59 bool line_is_ended = false;
60 for (size_t i = 0; i < nBytesAval && i < MAX_NMEA_LINE_LENGTH; i++)
61 {
62 const char val = static_cast<char>(m_rx_buffer.peek(i));
63 if (val == '\r' || val == '\n')
64 {
65 line_is_ended = true;
66 break;
67 }
68 line.push_back(val);
69 }
70 if (line_is_ended)
71 {
72 // Pop from buffer:
73 for (size_t i = 0; i < line.size(); i++) m_rx_buffer.pop();
74
75 // Parse:
76 const bool did_have_gga = m_parsed_messages->has_GGA_datum();
77 if (CGPSInterface::parse_NMEA(line, *m_parsed_messages, false /*verbose*/))
78 {
79 // Parsers must set only the part of the msg type:
80 m_parsed_messages->sensorLabel = "NMEA";
81
82 // Save GGA cache (useful for NTRIP,...)
83 const bool now_has_gga = m_parsed_messages->has_GGA_datum();
84 if (now_has_gga && !did_have_gga)
85 {
86 m_last_GGA = line;
87 }
88 }
89 else
90 {
91 if (m_verbose)
92 std::cerr << "[CGPSInterface::implement_parser_NMEA] Line "
93 "of unknown format ignored: `"
94 << line << "`\n";
95 }
96 return true;
97 }
98 else
99 {
100 // We still need to wait for more data to be read:
101 out_minimum_rx_buf_to_decide = nBytesAval + 1;
102 return true;
103 }
104 }
105
106 /* -----------------------------------------------------
107 parse_NMEA
108 ----------------------------------------------------- */
109 bool CGPSInterface::parse_NMEA(
110 const std::string& s, mrpt::obs::CObservationGPS& out_obs, const bool verbose)
111 {
112 // For building complete date+time in msgs without a date.
113 static mrpt::system::TTimeStamp last_known_date = mrpt::Clock::now();
114 static mrpt::system::TTimeStamp last_known_time = mrpt::Clock::now();
115
116 if (verbose)
117 {
118 std::cout << "[CGPSInterface] GPS raw string: " << s << endl;
119 }
120
121 // Firstly! If the string does not start with "$GP" it is not valid:
122 if (s.size() < 7)
123 {
124 return false;
125 }
126 if (s[0] != '$')
127 {
128 return false;
129 }
130
131 std::vector<std::string> lstTokens;
132 mrpt::system::tokenize(s, "*,\t\r\n", lstTokens, false /* do not skip blank tokens */);
133 if (lstTokens.size() < 3)
134 {
135 return false;
136 }
137
138 for (auto& lstToken : lstTokens)
139 {
140 lstToken = mrpt::system::trim(lstToken); // Trim whitespaces
141 }
142
143 bool parsed_ok = false;
144
145 // Remove talker ID "$xxGGA" ==> "GGA", but extract it first:
146 if (lstTokens[0].size() > 3)
147 {
148 const std::string talker = lstTokens[0].substr(1, 2); // e.g. "GP", "GL", "GN"
149
150 // Only set if not already set by a prior sentence in this observation
151 if (out_obs.gnss_service_mask == GnssService::NONE)
152 {
153 if (talker == "GP")
154 {
155 out_obs.gnss_service_mask = GnssService::GPS;
156 }
157 else if (talker == "GL")
158 {
159 out_obs.gnss_service_mask = GnssService::GLONASS;
160 }
161 else if (talker == "GA")
162 {
163 out_obs.gnss_service_mask = GnssService::GALILEO;
164 }
165 else if (talker == "GB" || talker == "BD")
166 {
167 out_obs.gnss_service_mask = GnssService::BEIDOU;
168 }
169 else if (talker == "GN")
170 {
171 // multi-constellation
172 out_obs.gnss_service_mask =
173 GnssService::GPS | GnssService::GLONASS | GnssService::BEIDOU | GnssService::GALILEO;
174 }
175 else if (talker == "QZ")
176 {
177 out_obs.gnss_service_mask = GnssService::GPS; // QZSS, GPS-compatible
178 }
179 }
180
181 lstTokens[0] = lstTokens[0].substr(3);
182 }
183
184 // Try to determine the kind of command:
185 if (lstTokens[0] == "GGA" && lstTokens.size() >= 13)
186 {
187 // ---------------------------------------------
188 // GGA
189 // ---------------------------------------------
190 bool all_fields_ok = true;
191 std::string token;
192
193 // Fill out the output structure:
194 gnss::Message_NMEA_GGA gga;
195
196 // Time:
197 token = lstTokens[1];
198 if (token.size() >= 6)
199 {
200 gga.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
201 gga.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
202 gga.fields.UTCTime.sec = atof(&(token.c_str()[4]));
203 }
204 else
205 all_fields_ok = false;
206
207 // Latitude:
208 token = lstTokens[2];
209 if (token.size() >= 4)
210 {
211 double lat = 10 * (token[0] - '0') + token[1] - '0';
212 lat += atof(&(token.c_str()[2])) / 60.0;
213 gga.fields.latitude_degrees = lat;
214 }
215 else
216 all_fields_ok = false;
217
218 // N/S:
219 token = lstTokens[3];
220 if (token.empty())
221 all_fields_ok = false;
222 else if (token[0] == 'S')
223 gga.fields.latitude_degrees = -gga.fields.latitude_degrees;
224
225 // Longitude:
226 token = lstTokens[4];
227 if (token.size() >= 5)
228 {
229 double lat = 100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
230 lat += atof(&(token.c_str()[3])) / 60.0;
231 gga.fields.longitude_degrees = lat;
232 }
233 else
234 all_fields_ok = false;
235
236 // E_W:
237 token = lstTokens[5];
238 if (token.empty())
239 all_fields_ok = false;
240 else if (token[0] == 'W')
241 gga.fields.longitude_degrees = -gga.fields.longitude_degrees;
242
243 // fix quality:
244 token = lstTokens[6];
245 if (!token.empty()) gga.fields.fix_quality = (unsigned char)atoi(token.c_str());
246
247 // sats:
248 token = lstTokens[7];
249 if (!token.empty()) gga.fields.satellitesUsed = (unsigned char)atoi(token.c_str());
250
251 // HDOP:
252 token = lstTokens[8];
253 if (!token.empty())
254 {
255 gga.fields.HDOP = (float)atof(token.c_str());
256 gga.fields.thereis_HDOP = true;
257 }
258
259 // Altitude:
260 token = lstTokens[9];
261 if (token.empty())
262 all_fields_ok = false;
263 else
264 gga.fields.altitude_meters = atof(token.c_str());
265
266 // Units of the altitude:
267 // token = lstTokens[10];
268 // ASSERT_(token == "M");
269
270 // Geoidal separation [B] (undulation)
271 token = lstTokens[11];
272 if (!token.empty()) gga.fields.geoidal_distance = atof(token.c_str());
273
274 // Units of the geoidal separation:
275 // token = lstTokens[12];
276 // ASSERT_(token == "M");
277
278 // Total altitude [A]+[B] and mmGPS Corrected total altitude
279 // Corr([A]+[B]):
280 gga.fields.orthometric_altitude = gga.fields.corrected_orthometric_altitude =
281 gga.fields.altitude_meters + gga.fields.geoidal_distance;
282
283 if (all_fields_ok)
284 {
285 out_obs.setMsg(gga);
286 out_obs.originalReceivedTimestamp = mrpt::Clock::now();
287 out_obs.timestamp = gga.fields.UTCTime.getAsTimestamp(last_known_date);
288 out_obs.has_satellite_timestamp = true;
289 }
290 parsed_ok = all_fields_ok;
291 }
292 else if (lstTokens[0] == "RMC" && lstTokens.size() >= 13)
293 {
294 // ---------------------------------------------
295 // GPRMC
296 // ---------------------------------------------
297 bool all_fields_ok = true;
298 std::string token;
299
300 // Fill out the output structure:
301 gnss::Message_NMEA_RMC rmc;
302
303 // Time:
304 token = lstTokens[1];
305 if (token.size() >= 6)
306 {
307 rmc.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
308 rmc.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
309 rmc.fields.UTCTime.sec = atof(&(token.c_str()[4]));
310 }
311 else
312 all_fields_ok = false;
313
314 // Valid?
315 token = lstTokens[2];
316 if (token.empty())
317 all_fields_ok = false;
318 else
319 rmc.fields.validity_char = token.c_str()[0];
320
321 // Latitude:
322 token = lstTokens[3];
323 if (token.size() >= 4)
324 {
325 double lat = 10 * (token[0] - '0') + token[1] - '0';
326 lat += atof(&(token.c_str()[2])) / 60.0;
327 rmc.fields.latitude_degrees = lat;
328 }
329 else
330 all_fields_ok = false;
331
332 // N/S:
333 token = lstTokens[4];
334 if (token.empty())
335 all_fields_ok = false;
336 else if (token[0] == 'S')
337 rmc.fields.latitude_degrees = -rmc.fields.latitude_degrees;
338
339 // Longitude:
340 token = lstTokens[5];
341 if (token.size() >= 5)
342 {
343 double lat = 100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
344 lat += atof(&(token.c_str()[3])) / 60.0;
345 rmc.fields.longitude_degrees = lat;
346 }
347 else
348 all_fields_ok = false;
349
350 // E/W:
351 token = lstTokens[6];
352 if (token.empty())
353 all_fields_ok = false;
354 else if (token[0] == 'W')
355 rmc.fields.longitude_degrees = -rmc.fields.longitude_degrees;
356
357 // Speed:
358 token = lstTokens[7];
359 if (!token.empty()) rmc.fields.speed_knots = atof(token.c_str());
360
361 // Direction:
362 token = lstTokens[8];
363 if (!token.empty()) rmc.fields.direction_degrees = atof(token.c_str());
364
365 // Date:
366 token = lstTokens[9];
367 if (token.size() >= 6)
368 {
369 rmc.fields.date_day = 10 * (token[0] - '0') + token[1] - '0';
370 rmc.fields.date_month = 10 * (token[2] - '0') + token[3] - '0';
371 rmc.fields.date_year = atoi(&(token.c_str()[4]));
372 }
373 else
374 all_fields_ok = false;
375
376 // Magnetic var
377 token = lstTokens[10];
378 if (token.size() >= 2)
379 {
380 rmc.fields.magnetic_dir = atof(token.c_str());
381 // E/W:
382 token = lstTokens[11];
383 if (token.empty())
384 all_fields_ok = false;
385 else if (token[0] == 'W')
386 rmc.fields.magnetic_dir = -rmc.fields.magnetic_dir;
387 }
388
389 // Mode ind.
390 if (lstTokens.size() >= 14)
391 {
392 // Only for NMEA 2.3
393 token = lstTokens[12];
394 if (token.empty())
395 all_fields_ok = false;
396 else
397 rmc.fields.positioning_mode = token.c_str()[0];
398 }
399 else
400 rmc.fields.positioning_mode = 'A'; // Default for older receiver
401
402 if (all_fields_ok)
403 {
404 out_obs.setMsg(rmc);
405 out_obs.originalReceivedTimestamp = mrpt::Clock::now();
406 out_obs.timestamp = rmc.fields.UTCTime.getAsTimestamp(rmc.getDateAsTimestamp());
407 last_known_date = rmc.getDateAsTimestamp();
408 last_known_time = out_obs.timestamp;
409 out_obs.has_satellite_timestamp = true;
410 }
411 parsed_ok = all_fields_ok;
412 }
413 else if (lstTokens[0] == "GLL" && lstTokens.size() >= 5)
414 {
415 // ---------------------------------------------
416 // GPGLL
417 // ---------------------------------------------
418 bool all_fields_ok = true;
419 std::string token;
420
421 // Fill out the output structure:
422 gnss::Message_NMEA_GLL gll;
423 // Latitude:
424 token = lstTokens[1];
425 if (token.size() >= 4)
426 {
427 double lat = 10 * (token[0] - '0') + token[1] - '0';
428 lat += atof(&(token.c_str()[2])) / 60.0;
429 gll.fields.latitude_degrees = lat;
430 }
431 else
432 all_fields_ok = false;
433
434 // N/S:
435 token = lstTokens[2];
436 if (token.empty())
437 all_fields_ok = false;
438 else if (token[0] == 'S')
439 gll.fields.latitude_degrees = -gll.fields.latitude_degrees;
440
441 // Longitude:
442 token = lstTokens[3];
443 if (token.size() >= 5)
444 {
445 double lat = 100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
446 lat += atof(&(token.c_str()[3])) / 60.0;
447 gll.fields.longitude_degrees = lat;
448 }
449 else
450 all_fields_ok = false;
451
452 // E/W:
453 token = lstTokens[4];
454 if (token.empty())
455 all_fields_ok = false;
456 else if (token[0] == 'W')
457 gll.fields.longitude_degrees = -gll.fields.longitude_degrees;
458
459 if (lstTokens.size() >= 7)
460 {
461 // Time:
462 token = lstTokens[5];
463 if (token.size() >= 6)
464 {
465 gll.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
466 gll.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
467 gll.fields.UTCTime.sec = atof(&(token.c_str()[4]));
468 }
469 else
470 all_fields_ok = false;
471
472 // Valid?
473 token = lstTokens[6];
474 if (token.empty())
475 all_fields_ok = false;
476 else
477 gll.fields.validity_char = token.c_str()[0];
478 }
479
480 if (all_fields_ok)
481 {
482 out_obs.setMsg(gll);
483 out_obs.originalReceivedTimestamp = mrpt::Clock::now();
484 out_obs.timestamp = gll.fields.UTCTime.getAsTimestamp(last_known_date);
485 last_known_time = out_obs.timestamp;
486 out_obs.has_satellite_timestamp = true;
487 }
488 parsed_ok = all_fields_ok;
489 }
490 else if (lstTokens[0] == "VTG" && lstTokens.size() >= 9)
491 {
492 // ---------------------------------------------
493 // GPVTG
494 // ---------------------------------------------
495 bool all_fields_ok = true;
496 std::string token;
497
498 // Fill out the output structure:
499 gnss::Message_NMEA_VTG vtg;
500
501 vtg.fields.true_track = atof(lstTokens[1].c_str());
502 vtg.fields.magnetic_track = atof(lstTokens[3].c_str());
503 vtg.fields.ground_speed_knots = atof(lstTokens[5].c_str());
504 vtg.fields.ground_speed_kmh = atof(lstTokens[7].c_str());
505
506 if (lstTokens[2] != "T" || lstTokens[4] != "M" || lstTokens[6] != "N" || lstTokens[8] != "K")
507 all_fields_ok = false;
508
509 if (all_fields_ok)
510 {
511 out_obs.setMsg(vtg);
512 out_obs.originalReceivedTimestamp = mrpt::Clock::now();
513 out_obs.timestamp = last_known_time;
514 out_obs.has_satellite_timestamp = false;
515 }
516 parsed_ok = all_fields_ok;
517 }
518 else if (lstTokens[0] == "ZDA" && lstTokens.size() >= 5)
519 {
520 // ---------------------------------------------
521 // GPZDA
522 // ---------------------------------------------
523 bool all_fields_ok = true;
524 std::string token;
525
526 // Fill out the output structure:
527 gnss::Message_NMEA_ZDA zda;
528 //$--ZDA,hhmmss.ss,xx,xx,xxxx,xx,xx
529 // hhmmss.ss = UTC
530 // xx = Day, 01 to 31
531 // xx = Month, 01 to 12
532 // xxxx = Year
533 // xx = Local zone description, 00 to +/- 13 hours
534 // xx = Local zone minutes description (same sign as hours)
535
536 // Time:
537 token = lstTokens[1];
538 if (token.size() >= 6)
539 {
540 zda.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
541 zda.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
542 zda.fields.UTCTime.sec = atof(&(token.c_str()[4]));
543 }
544 else
545 all_fields_ok = false;
546
547 // Day:
548 token = lstTokens[2];
549 if (!token.empty()) zda.fields.date_day = atoi(token.c_str());
550 // Month:
551 token = lstTokens[3];
552 if (!token.empty()) zda.fields.date_month = atoi(token.c_str());
553 // Year:
554 token = lstTokens[4];
555 if (!token.empty()) zda.fields.date_year = atoi(token.c_str());
556
557 if (all_fields_ok)
558 {
559 out_obs.setMsg(zda);
560 out_obs.originalReceivedTimestamp = mrpt::Clock::now();
561 try
562 {
563 out_obs.timestamp = zda.getDateTimeAsTimestamp();
564 last_known_date = zda.getDateAsTimestamp();
565 out_obs.has_satellite_timestamp = true;
566 last_known_time = out_obs.timestamp;
567 }
568 catch (...)
569 {
570 // Invalid date:
571 out_obs.timestamp = out_obs.originalReceivedTimestamp;
572 }
573 }
574 parsed_ok = all_fields_ok;
575 }
576 else if (lstTokens[0] == "GSA" && lstTokens.size() >= 18)
577 {
578 // ---------------------------------------------
579 // GSA
580 // ---------------------------------------------
581 bool all_fields_ok = true;
582 std::string token;
583
584 // Fill out the output structure:
585 gnss::Message_NMEA_GSA gsa;
586 // $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
587 // Where:
588 // GSA Satellite status
589 // A Auto selection of 2D or 3D fix (M = manual)
590 // 3 3D fix - values include: 1 = no fix
591 // 2 = 2D fix
592 // 3 = 3D fix
593 // 04,05... PRNs of satellites used for fix (space for 12)
594 // 2.5 PDOP (dilution of precision)
595 // 1.3 Horizontal dilution of precision (HDOP)
596 // 2.1 Vertical dilution of precision (VDOP)
597 // *39 the checksum data, always begins with *
598 token = lstTokens[1];
599 if (!token.empty())
600 gsa.fields.auto_selection_fix = token[0];
601 else
602 all_fields_ok = false;
603
604 token = lstTokens[2];
605 if (!token.empty())
606 gsa.fields.fix_2D_3D = token[0];
607 else
608 all_fields_ok = false;
609
610 for (int i = 0; i < 12; i++)
611 {
612 token = lstTokens[3 + i];
613 if (token.size() > 0) gsa.fields.PRNs[i][0] = token[0];
614 if (token.size() > 1) gsa.fields.PRNs[i][1] = token[1];
615 }
616 // PDOP:
617 gsa.fields.PDOP = ::atof(lstTokens[3 + 12 + 0].data());
618 gsa.fields.HDOP = ::atof(lstTokens[3 + 12 + 1].data());
619 gsa.fields.VDOP = ::atof(lstTokens[3 + 12 + 2].data());
620
621 if (all_fields_ok)
622 {
623 out_obs.setMsg(gsa);
624 out_obs.originalReceivedTimestamp = mrpt::Clock::now();
625 }
626 parsed_ok = all_fields_ok;
627 }
628 else
629 {
630 // other commands?
631 }
632
633 return parsed_ok;
634 }