MRPT  2.0.4
CGPSInterface_parser_NMEA.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 
13 #include <mrpt/system/filesystem.h>
14 #include <mrpt/system/os.h>
15 #include <iostream>
16 
17 using namespace mrpt::hwdrivers;
18 using namespace mrpt::obs;
19 using namespace mrpt::system;
20 using namespace std;
21 
22 const size_t MAX_NMEA_LINE_LENGTH = 1024;
23 
24 bool CGPSInterface::implement_parser_NMEA(size_t& out_minimum_rx_buf_to_decide)
25 {
26  out_minimum_rx_buf_to_decide = 3;
27 
28  if (m_rx_buffer.size() < 3) return true; // no need to skip a byte
29 
30  const size_t nBytesAval = m_rx_buffer.size(); // Available for read
31 
32  // If the string does not start with "$GP" it is not valid:
33  uint8_t buf[3]; // peek_buffer
34  m_rx_buffer.peek_many(&buf[0], 3);
35 
36  // Known "talkers": Baidu, Galileo, GPS, GLONASS, etc.
37  // Ref: https://gpsd.gitlab.io/gpsd/NMEA.html
38  static const std::array<const char*, 12> known_prefixes = {
39  "BD", "CD", "EC", "GA", "GB", "GL", "GN", "GP", "II", "IN", "LC", "QZ"};
40 
41  bool recognized = false;
42  if (buf[0] == '$')
43  {
44  for (const char* prefix : known_prefixes)
45  {
46  if (buf[1] == prefix[0] && buf[2] == prefix[1])
47  {
48  recognized = true;
49  break;
50  }
51  }
52  }
53  // Not the start of a NMEA string, skip 1 char:
54  if (!recognized) return false;
55 
56  // It starts OK: try to find the end of the line
57  std::string line;
58  bool line_is_ended = false;
59  for (size_t i = 0; i < nBytesAval && i < MAX_NMEA_LINE_LENGTH; i++)
60  {
61  const char val = static_cast<char>(m_rx_buffer.peek(i));
62  if (val == '\r' || val == '\n')
63  {
64  line_is_ended = true;
65  break;
66  }
67  line.push_back(val);
68  }
69  if (line_is_ended)
70  {
71  // Pop from buffer:
72  for (size_t i = 0; i < line.size(); i++) m_rx_buffer.pop();
73 
74  // Parse:
75  const bool did_have_gga = m_just_parsed_messages.has_GGA_datum();
77  line, m_just_parsed_messages, false /*verbose*/))
78  {
79  // Parsers must set only the part of the msg type:
80  m_just_parsed_messages.sensorLabel = "NMEA";
81 
82  // Save GGA cache (useful for NTRIP,...)
83  const bool now_has_gga = m_just_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 ----------------------------------------------------- */
110  const std::string& s, mrpt::obs::CObservationGPS& out_obs,
111  const bool verbose)
112 {
113  static mrpt::system::TTimeStamp last_known_date =
114  mrpt::system::now(); // For building complete date+time in msgs without
115  // a date.
116  static mrpt::system::TTimeStamp last_known_time = mrpt::system::now();
117 
118  if (verbose) cout << "[CGPSInterface] GPS raw string: " << s << endl;
119 
120  // Firstly! If the string does not start with "$GP" it is not valid:
121  if (s.size() < 7) return false;
122  if (s[0] != '$') return false;
123 
124  std::vector<std::string> lstTokens;
126  s, "*,\t\r\n", lstTokens, false /* do not skip blank tokens */);
127  if (lstTokens.size() < 3) return false;
128 
129  for (auto& lstToken : lstTokens)
130  lstToken = mrpt::system::trim(lstToken); // Trim whitespaces
131 
132  bool parsed_ok = false;
133 
134  // Remove talker ID "$xxGGA" ==> "GGA"
135  if (lstTokens[0].size() > 3) lstTokens[0] = lstTokens[0].substr(3);
136 
137  // Try to determine the kind of command:
138  if (lstTokens[0] == "GGA" && lstTokens.size() >= 13)
139  {
140  // ---------------------------------------------
141  // GGA
142  // ---------------------------------------------
143  bool all_fields_ok = true;
144  std::string token;
145 
146  // Fill out the output structure:
148 
149  // Time:
150  token = lstTokens[1];
151  if (token.size() >= 6)
152  {
153  gga.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
154  gga.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
155  gga.fields.UTCTime.sec = atof(&(token.c_str()[4]));
156  }
157  else
158  all_fields_ok = false;
159 
160  // Latitude:
161  token = lstTokens[2];
162  if (token.size() >= 4)
163  {
164  double lat = 10 * (token[0] - '0') + token[1] - '0';
165  lat += atof(&(token.c_str()[2])) / 60.0;
167  }
168  else
169  all_fields_ok = false;
170 
171  // N/S:
172  token = lstTokens[3];
173  if (token.empty())
174  all_fields_ok = false;
175  else if (token[0] == 'S')
177 
178  // Longitude:
179  token = lstTokens[4];
180  if (token.size() >= 5)
181  {
182  double lat =
183  100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
184  lat += atof(&(token.c_str()[3])) / 60.0;
186  }
187  else
188  all_fields_ok = false;
189 
190  // E_W:
191  token = lstTokens[5];
192  if (token.empty())
193  all_fields_ok = false;
194  else if (token[0] == 'W')
196 
197  // fix quality:
198  token = lstTokens[6];
199  if (!token.empty())
200  gga.fields.fix_quality = (unsigned char)atoi(token.c_str());
201 
202  // sats:
203  token = lstTokens[7];
204  if (!token.empty())
205  gga.fields.satellitesUsed = (unsigned char)atoi(token.c_str());
206 
207  // HDOP:
208  token = lstTokens[8];
209  if (!token.empty())
210  {
211  gga.fields.HDOP = (float)atof(token.c_str());
212  gga.fields.thereis_HDOP = true;
213  }
214 
215  // Altitude:
216  token = lstTokens[9];
217  if (token.empty())
218  all_fields_ok = false;
219  else
220  gga.fields.altitude_meters = atof(token.c_str());
221 
222  // Units of the altitude:
223  // token = lstTokens[10];
224  // ASSERT_(token == "M");
225 
226  // Geoidal separation [B] (undulation)
227  token = lstTokens[11];
228  if (!token.empty()) gga.fields.geoidal_distance = atof(token.c_str());
229 
230  // Units of the geoidal separation:
231  // token = lstTokens[12];
232  // ASSERT_(token == "M");
233 
234  // Total altitude [A]+[B] and mmGPS Corrected total altitude
235  // Corr([A]+[B]):
239 
240  if (all_fields_ok)
241  {
242  out_obs.setMsg(gga);
244  out_obs.timestamp =
245  gga.fields.UTCTime.getAsTimestamp(last_known_date);
246  out_obs.has_satellite_timestamp = true;
247  }
248  parsed_ok = all_fields_ok;
249  }
250  else if (lstTokens[0] == "RMC" && lstTokens.size() >= 13)
251  {
252  // ---------------------------------------------
253  // GPRMC
254  // ---------------------------------------------
255  bool all_fields_ok = true;
256  std::string token;
257 
258  // Fill out the output structure:
260 
261  // Time:
262  token = lstTokens[1];
263  if (token.size() >= 6)
264  {
265  rmc.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
266  rmc.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
267  rmc.fields.UTCTime.sec = atof(&(token.c_str()[4]));
268  }
269  else
270  all_fields_ok = false;
271 
272  // Valid?
273  token = lstTokens[2];
274  if (token.empty())
275  all_fields_ok = false;
276  else
277  rmc.fields.validity_char = token.c_str()[0];
278 
279  // Latitude:
280  token = lstTokens[3];
281  if (token.size() >= 4)
282  {
283  double lat = 10 * (token[0] - '0') + token[1] - '0';
284  lat += atof(&(token.c_str()[2])) / 60.0;
286  }
287  else
288  all_fields_ok = false;
289 
290  // N/S:
291  token = lstTokens[4];
292  if (token.empty())
293  all_fields_ok = false;
294  else if (token[0] == 'S')
296 
297  // Longitude:
298  token = lstTokens[5];
299  if (token.size() >= 5)
300  {
301  double lat =
302  100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
303  lat += atof(&(token.c_str()[3])) / 60.0;
305  }
306  else
307  all_fields_ok = false;
308 
309  // E/W:
310  token = lstTokens[6];
311  if (token.empty())
312  all_fields_ok = false;
313  else if (token[0] == 'W')
315 
316  // Speed:
317  token = lstTokens[7];
318  if (!token.empty()) rmc.fields.speed_knots = atof(token.c_str());
319 
320  // Direction:
321  token = lstTokens[8];
322  if (!token.empty()) rmc.fields.direction_degrees = atof(token.c_str());
323 
324  // Date:
325  token = lstTokens[9];
326  if (token.size() >= 6)
327  {
328  rmc.fields.date_day = 10 * (token[0] - '0') + token[1] - '0';
329  rmc.fields.date_month = 10 * (token[2] - '0') + token[3] - '0';
330  rmc.fields.date_year = atoi(&(token.c_str()[4]));
331  }
332  else
333  all_fields_ok = false;
334 
335  // Magnetic var
336  token = lstTokens[10];
337  if (token.size() >= 2)
338  {
339  rmc.fields.magnetic_dir = atof(token.c_str());
340  // E/W:
341  token = lstTokens[11];
342  if (token.empty())
343  all_fields_ok = false;
344  else if (token[0] == 'W')
346  }
347 
348  // Mode ind.
349  if (lstTokens.size() >= 14)
350  {
351  // Only for NMEA 2.3
352  token = lstTokens[12];
353  if (token.empty())
354  all_fields_ok = false;
355  else
356  rmc.fields.positioning_mode = token.c_str()[0];
357  }
358  else
359  rmc.fields.positioning_mode = 'A'; // Default for older receiver
360 
361  if (all_fields_ok)
362  {
363  out_obs.setMsg(rmc);
365  out_obs.timestamp =
367  last_known_date = rmc.getDateAsTimestamp();
368  last_known_time = out_obs.timestamp;
369  out_obs.has_satellite_timestamp = true;
370  }
371  parsed_ok = all_fields_ok;
372  }
373  else if (lstTokens[0] == "GLL" && lstTokens.size() >= 5)
374  {
375  // ---------------------------------------------
376  // GPGLL
377  // ---------------------------------------------
378  bool all_fields_ok = true;
379  std::string token;
380 
381  // Fill out the output structure:
383  // Latitude:
384  token = lstTokens[1];
385  if (token.size() >= 4)
386  {
387  double lat = 10 * (token[0] - '0') + token[1] - '0';
388  lat += atof(&(token.c_str()[2])) / 60.0;
390  }
391  else
392  all_fields_ok = false;
393 
394  // N/S:
395  token = lstTokens[2];
396  if (token.empty())
397  all_fields_ok = false;
398  else if (token[0] == 'S')
400 
401  // Longitude:
402  token = lstTokens[3];
403  if (token.size() >= 5)
404  {
405  double lat =
406  100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
407  lat += atof(&(token.c_str()[3])) / 60.0;
409  }
410  else
411  all_fields_ok = false;
412 
413  // E/W:
414  token = lstTokens[4];
415  if (token.empty())
416  all_fields_ok = false;
417  else if (token[0] == 'W')
419 
420  if (lstTokens.size() >= 7)
421  {
422  // Time:
423  token = lstTokens[5];
424  if (token.size() >= 6)
425  {
426  gll.fields.UTCTime.hour =
427  10 * (token[0] - '0') + token[1] - '0';
428  gll.fields.UTCTime.minute =
429  10 * (token[2] - '0') + token[3] - '0';
430  gll.fields.UTCTime.sec = atof(&(token.c_str()[4]));
431  }
432  else
433  all_fields_ok = false;
434 
435  // Valid?
436  token = lstTokens[6];
437  if (token.empty())
438  all_fields_ok = false;
439  else
440  gll.fields.validity_char = token.c_str()[0];
441  }
442 
443  if (all_fields_ok)
444  {
445  out_obs.setMsg(gll);
447  out_obs.timestamp =
448  gll.fields.UTCTime.getAsTimestamp(last_known_date);
449  last_known_time = out_obs.timestamp;
450  out_obs.has_satellite_timestamp = true;
451  }
452  parsed_ok = all_fields_ok;
453  }
454  else if (lstTokens[0] == "VTG" && lstTokens.size() >= 9)
455  {
456  // ---------------------------------------------
457  // GPVTG
458  // ---------------------------------------------
459  bool all_fields_ok = true;
460  std::string token;
461 
462  // Fill out the output structure:
464 
465  vtg.fields.true_track = atof(lstTokens[1].c_str());
466  vtg.fields.magnetic_track = atof(lstTokens[3].c_str());
467  vtg.fields.ground_speed_knots = atof(lstTokens[5].c_str());
468  vtg.fields.ground_speed_kmh = atof(lstTokens[7].c_str());
469 
470  if (lstTokens[2] != "T" || lstTokens[4] != "M" || lstTokens[6] != "N" ||
471  lstTokens[8] != "K")
472  all_fields_ok = false;
473 
474  if (all_fields_ok)
475  {
476  out_obs.setMsg(vtg);
478  out_obs.timestamp = last_known_time;
479  out_obs.has_satellite_timestamp = false;
480  }
481  parsed_ok = all_fields_ok;
482  }
483  else if (lstTokens[0] == "ZDA" && lstTokens.size() >= 5)
484  {
485  // ---------------------------------------------
486  // GPZDA
487  // ---------------------------------------------
488  bool all_fields_ok = true;
489  std::string token;
490 
491  // Fill out the output structure:
493  //$--ZDA,hhmmss.ss,xx,xx,xxxx,xx,xx
494  // hhmmss.ss = UTC
495  // xx = Day, 01 to 31
496  // xx = Month, 01 to 12
497  // xxxx = Year
498  // xx = Local zone description, 00 to +/- 13 hours
499  // xx = Local zone minutes description (same sign as hours)
500 
501  // Time:
502  token = lstTokens[1];
503  if (token.size() >= 6)
504  {
505  zda.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
506  zda.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
507  zda.fields.UTCTime.sec = atof(&(token.c_str()[4]));
508  }
509  else
510  all_fields_ok = false;
511 
512  // Day:
513  token = lstTokens[2];
514  if (!token.empty()) zda.fields.date_day = atoi(token.c_str());
515  // Month:
516  token = lstTokens[3];
517  if (!token.empty()) zda.fields.date_month = atoi(token.c_str());
518  // Year:
519  token = lstTokens[4];
520  if (!token.empty()) zda.fields.date_year = atoi(token.c_str());
521 
522  if (all_fields_ok)
523  {
524  out_obs.setMsg(zda);
526  try
527  {
528  out_obs.timestamp = zda.getDateTimeAsTimestamp();
529  last_known_date = zda.getDateAsTimestamp();
530  out_obs.has_satellite_timestamp = true;
531  last_known_time = out_obs.timestamp;
532  }
533  catch (...)
534  {
535  // Invalid date:
536  out_obs.timestamp = out_obs.originalReceivedTimestamp;
537  }
538  }
539  parsed_ok = all_fields_ok;
540  }
541  else if (lstTokens[0] == "GSA" && lstTokens.size() >= 18)
542  {
543  // ---------------------------------------------
544  // GSA
545  // ---------------------------------------------
546  bool all_fields_ok = true;
547  std::string token;
548 
549  // Fill out the output structure:
551  // $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
552  // Where:
553  // GSA Satellite status
554  // A Auto selection of 2D or 3D fix (M = manual)
555  // 3 3D fix - values include: 1 = no fix
556  // 2 = 2D fix
557  // 3 = 3D fix
558  // 04,05... PRNs of satellites used for fix (space for 12)
559  // 2.5 PDOP (dilution of precision)
560  // 1.3 Horizontal dilution of precision (HDOP)
561  // 2.1 Vertical dilution of precision (VDOP)
562  // *39 the checksum data, always begins with *
563  token = lstTokens[1];
564  if (!token.empty())
565  gsa.fields.auto_selection_fix = token[0];
566  else
567  all_fields_ok = false;
568 
569  token = lstTokens[2];
570  if (!token.empty())
571  gsa.fields.fix_2D_3D = token[0];
572  else
573  all_fields_ok = false;
574 
575  for (int i = 0; i < 12; i++)
576  {
577  token = lstTokens[3 + i];
578  if (token.size() > 0) gsa.fields.PRNs[i][0] = token[0];
579  if (token.size() > 1) gsa.fields.PRNs[i][1] = token[1];
580  }
581  // PDOP:
582  gsa.fields.PDOP = ::atof(lstTokens[3 + 12 + 0].data());
583  gsa.fields.HDOP = ::atof(lstTokens[3 + 12 + 1].data());
584  gsa.fields.VDOP = ::atof(lstTokens[3 + 12 + 2].data());
585 
586  if (all_fields_ok)
587  {
588  out_obs.setMsg(gsa);
590  }
591  parsed_ok = all_fields_ok;
592  }
593  else
594  {
595  // other commands?
596  }
597 
598  return parsed_ok;
599 }
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
uint8_t fix_quality
NMEA standard values: 0 = invalid, 1 = GPS fix (SPS), 2 = DGPS fix, 3 = PPS fix, 4 = Real Time Kinema...
content_t fields
Message content, accesible by individual fields.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
mrpt::system::TTimeStamp getAsTimestamp(const mrpt::system::TTimeStamp &date) const
Build an MRPT timestamp with the hour/minute/sec of this structure and the date from the given timest...
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
size_t size(const MATRIXLIKE &m, const int dim)
mrpt::system::TTimeStamp originalReceivedTimestamp
The local computer-based timestamp based on the reception of the message in the computer.
content_t fields
Message content, accesible by individual fields.
content_t fields
Message content, accesible by individual fields.
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
char fix_2D_3D
1: no fix, 2: 2D fix, 3: 3D fix
const size_t MAX_NMEA_LINE_LENGTH
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
content_t fields
Message content, accesible by individual fields.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
STL namespace.
int8_t validity_char
This will be: &#39;A&#39;=OK or &#39;V&#39;=void.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
double orthometric_altitude
The measured orthometric altitude, in meters (A)+(B).
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
double altitude_meters
The measured altitude, in meters (A).
bool thereis_HDOP
This states whether to take into account the value in the HDOP field.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
double corrected_orthometric_altitude
The corrected (only for TopCon mmGPS) orthometric altitude, in meters mmGPS(A+B). ...
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
bool implement_parser_NMEA(size_t &out_minimum_rx_buf_to_decide)
uint8_t date_day
Date: day (1-31), month (1-12), two-digits year (00-99)
content_t fields
Message content, accesible by individual fields.
float HDOP
The HDOP (Horizontal Dilution of Precision) as returned by the sensor.
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
This namespace contains representation of robot actions and observations.
int val
Definition: mrpt_jpeglib.h:957
double lat
[deg], [deg], hgt over sea level[m]
bool has_satellite_timestamp
If true, CObservation::timestamp has been generated from accurate satellite clock.
static bool parse_NMEA(const std::string &cmd_line, mrpt::obs::CObservationGPS &out_obs, const bool verbose=false)
Parses one line of NMEA data from a GPS receiver, and writes the recognized fields (if any) into an o...
double geoidal_distance
Undulation: Difference between the measured altitude and the geoid, in meters (B).
double magnetic_dir
Magnetic variation direction (East:+, West:-)
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
mrpt::system::TTimeStamp getDateTimeAsTimestamp() const
Build an MRPT UTC timestamp with the year/month/day + hour/minute/sec of this observation.
void setMsg(const MSG_CLASS &msg)
Stores a message in the list messages, making a copy of the passed object.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
double direction_degrees
Measured speed direction (in degrees)
char positioning_mode
&#39;A&#39;: Autonomous, &#39;D&#39;: Differential, &#39;N&#39;: Not valid, &#39;E&#39;: Estimated, &#39;M&#39;: Manual
content_t fields
Message content, accesible by individual fields.
std::string trim(const std::string &str)
Removes leading and trailing spaces.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
int8_t validity_char
This will be: &#39;A&#39;=OK or &#39;V&#39;=void.
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
for(unsigned int i=0;i< NUM_IMGS;i++)
static struct FontData data
Definition: gltext.cpp:144



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020