MRPT  1.9.9
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-2019, 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 peek_buffer[3];
34  m_rx_buffer.peek_many(&peek_buffer[0], 3);
35  if (peek_buffer[0] != '$' || peek_buffer[1] != 'G' || peek_buffer[2] != 'P')
36  {
37  // Not the start of a NMEA string, skip 1 char:
38  return false;
39  }
40  else
41  {
42  // It starts OK: try to find the end of the line
43  std::string line;
44  bool line_is_ended = false;
45  for (size_t i = 0; i < nBytesAval && i < MAX_NMEA_LINE_LENGTH; i++)
46  {
47  const char val = static_cast<char>(m_rx_buffer.peek(i));
48  if (val == '\r' || val == '\n')
49  {
50  line_is_ended = true;
51  break;
52  }
53  line.push_back(val);
54  }
55  if (line_is_ended)
56  {
57  // Pop from buffer:
58  for (size_t i = 0; i < line.size(); i++) m_rx_buffer.pop();
59 
60  // Parse:
61  const bool did_have_gga = m_just_parsed_messages.has_GGA_datum();
63  line, m_just_parsed_messages, false /*verbose*/))
64  {
65  // Parsers must set only the part of the msg type:
66  m_just_parsed_messages.sensorLabel = "NMEA";
67 
68  // Save GGA cache (useful for NTRIP,...)
69  const bool now_has_gga = m_just_parsed_messages.has_GGA_datum();
70  if (now_has_gga && !did_have_gga)
71  {
72  m_last_GGA = line;
73  }
74  }
75  else
76  {
77  if (m_verbose)
78  std::cerr << "[CGPSInterface::implement_parser_NMEA] Line "
79  "of unknown format ignored: `"
80  << line << "`\n";
81  }
82  return true;
83  }
84  else
85  {
86  // We still need to wait for more data to be read:
87  out_minimum_rx_buf_to_decide = nBytesAval + 1;
88  return true;
89  }
90  }
91 }
92 
93 /* -----------------------------------------------------
94  parse_NMEA
95 ----------------------------------------------------- */
97  const std::string& s, mrpt::obs::CObservationGPS& out_obs,
98  const bool verbose)
99 {
100  static mrpt::system::TTimeStamp last_known_date =
101  mrpt::system::now(); // For building complete date+time in msgs without
102  // a date.
103  static mrpt::system::TTimeStamp last_known_time = mrpt::system::now();
104 
105  if (verbose) cout << "[CGPSInterface] GPS raw string: " << s << endl;
106 
107  // Firstly! If the string does not start with "$GP" it is not valid:
108  if (s.size() < 7) return false;
109  if (s[0] != '$' || s[1] != 'G') return false;
110 
111  std::vector<std::string> lstTokens;
113  s, "*,\t\r\n", lstTokens, false /* do not skip blank tokens */);
114  if (lstTokens.size() < 3) return false;
115 
116  for (auto& lstToken : lstTokens)
117  lstToken = mrpt::system::trim(lstToken); // Trim whitespaces
118 
119  bool parsed_ok = false;
120  // Try to determine the kind of command:
121  if (lstTokens[0] == "$GPGGA" && lstTokens.size() >= 13)
122  {
123  // ---------------------------------------------
124  // GGA
125  // ---------------------------------------------
126  bool all_fields_ok = true;
127  std::string token;
128 
129  // Fill out the output structure:
131 
132  // Time:
133  token = lstTokens[1];
134  if (token.size() >= 6)
135  {
136  gga.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
137  gga.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
138  gga.fields.UTCTime.sec = atof(&(token.c_str()[4]));
139  }
140  else
141  all_fields_ok = false;
142 
143  // Latitude:
144  token = lstTokens[2];
145  if (token.size() >= 4)
146  {
147  double lat = 10 * (token[0] - '0') + token[1] - '0';
148  lat += atof(&(token.c_str()[2])) / 60.0;
150  }
151  else
152  all_fields_ok = false;
153 
154  // N/S:
155  token = lstTokens[3];
156  if (token.empty())
157  all_fields_ok = false;
158  else if (token[0] == 'S')
160 
161  // Longitude:
162  token = lstTokens[4];
163  if (token.size() >= 5)
164  {
165  double lat =
166  100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
167  lat += atof(&(token.c_str()[3])) / 60.0;
169  }
170  else
171  all_fields_ok = false;
172 
173  // E_W:
174  token = lstTokens[5];
175  if (token.empty())
176  all_fields_ok = false;
177  else if (token[0] == 'W')
179 
180  // fix quality:
181  token = lstTokens[6];
182  if (!token.empty())
183  gga.fields.fix_quality = (unsigned char)atoi(token.c_str());
184 
185  // sats:
186  token = lstTokens[7];
187  if (!token.empty())
188  gga.fields.satellitesUsed = (unsigned char)atoi(token.c_str());
189 
190  // HDOP:
191  token = lstTokens[8];
192  if (!token.empty())
193  {
194  gga.fields.HDOP = (float)atof(token.c_str());
195  gga.fields.thereis_HDOP = true;
196  }
197 
198  // Altitude:
199  token = lstTokens[9];
200  if (token.empty())
201  all_fields_ok = false;
202  else
203  gga.fields.altitude_meters = atof(token.c_str());
204 
205  // Units of the altitude:
206  // token = lstTokens[10];
207  // ASSERT_(token == "M");
208 
209  // Geoidal separation [B] (undulation)
210  token = lstTokens[11];
211  if (!token.empty()) gga.fields.geoidal_distance = atof(token.c_str());
212 
213  // Units of the geoidal separation:
214  // token = lstTokens[12];
215  // ASSERT_(token == "M");
216 
217  // Total altitude [A]+[B] and mmGPS Corrected total altitude
218  // Corr([A]+[B]):
222 
223  if (all_fields_ok)
224  {
225  out_obs.setMsg(gga);
227  out_obs.timestamp =
228  gga.fields.UTCTime.getAsTimestamp(last_known_date);
229  out_obs.has_satellite_timestamp = true;
230  }
231  parsed_ok = all_fields_ok;
232  }
233  else if (lstTokens[0] == "$GPRMC" && lstTokens.size() >= 13)
234  {
235  // ---------------------------------------------
236  // GPRMC
237  // ---------------------------------------------
238  bool all_fields_ok = true;
239  std::string token;
240 
241  // Fill out the output structure:
243 
244  // Time:
245  token = lstTokens[1];
246  if (token.size() >= 6)
247  {
248  rmc.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
249  rmc.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
250  rmc.fields.UTCTime.sec = atof(&(token.c_str()[4]));
251  }
252  else
253  all_fields_ok = false;
254 
255  // Valid?
256  token = lstTokens[2];
257  if (token.empty())
258  all_fields_ok = false;
259  else
260  rmc.fields.validity_char = token.c_str()[0];
261 
262  // Latitude:
263  token = lstTokens[3];
264  if (token.size() >= 4)
265  {
266  double lat = 10 * (token[0] - '0') + token[1] - '0';
267  lat += atof(&(token.c_str()[2])) / 60.0;
269  }
270  else
271  all_fields_ok = false;
272 
273  // N/S:
274  token = lstTokens[4];
275  if (token.empty())
276  all_fields_ok = false;
277  else if (token[0] == 'S')
279 
280  // Longitude:
281  token = lstTokens[5];
282  if (token.size() >= 5)
283  {
284  double lat =
285  100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
286  lat += atof(&(token.c_str()[3])) / 60.0;
288  }
289  else
290  all_fields_ok = false;
291 
292  // E/W:
293  token = lstTokens[6];
294  if (token.empty())
295  all_fields_ok = false;
296  else if (token[0] == 'W')
298 
299  // Speed:
300  token = lstTokens[7];
301  if (!token.empty()) rmc.fields.speed_knots = atof(token.c_str());
302 
303  // Direction:
304  token = lstTokens[8];
305  if (!token.empty()) rmc.fields.direction_degrees = atof(token.c_str());
306 
307  // Date:
308  token = lstTokens[9];
309  if (token.size() >= 6)
310  {
311  rmc.fields.date_day = 10 * (token[0] - '0') + token[1] - '0';
312  rmc.fields.date_month = 10 * (token[2] - '0') + token[3] - '0';
313  rmc.fields.date_year = atoi(&(token.c_str()[4]));
314  }
315  else
316  all_fields_ok = false;
317 
318  // Magnetic var
319  token = lstTokens[10];
320  if (token.size() >= 2)
321  {
322  rmc.fields.magnetic_dir = atof(token.c_str());
323  // E/W:
324  token = lstTokens[11];
325  if (token.empty())
326  all_fields_ok = false;
327  else if (token[0] == 'W')
329  }
330 
331  // Mode ind.
332  if (lstTokens.size() >= 14)
333  {
334  // Only for NMEA 2.3
335  token = lstTokens[12];
336  if (token.empty())
337  all_fields_ok = false;
338  else
339  rmc.fields.positioning_mode = token.c_str()[0];
340  }
341  else
342  rmc.fields.positioning_mode = 'A'; // Default for older receiver
343 
344  if (all_fields_ok)
345  {
346  out_obs.setMsg(rmc);
348  out_obs.timestamp =
350  last_known_date = rmc.getDateAsTimestamp();
351  last_known_time = out_obs.timestamp;
352  out_obs.has_satellite_timestamp = true;
353  }
354  parsed_ok = all_fields_ok;
355  }
356  else if (lstTokens[0] == "$GPGLL" && lstTokens.size() >= 5)
357  {
358  // ---------------------------------------------
359  // GPGLL
360  // ---------------------------------------------
361  bool all_fields_ok = true;
362  std::string token;
363 
364  // Fill out the output structure:
366  // Latitude:
367  token = lstTokens[1];
368  if (token.size() >= 4)
369  {
370  double lat = 10 * (token[0] - '0') + token[1] - '0';
371  lat += atof(&(token.c_str()[2])) / 60.0;
373  }
374  else
375  all_fields_ok = false;
376 
377  // N/S:
378  token = lstTokens[2];
379  if (token.empty())
380  all_fields_ok = false;
381  else if (token[0] == 'S')
383 
384  // Longitude:
385  token = lstTokens[3];
386  if (token.size() >= 5)
387  {
388  double lat =
389  100 * (token[0] - '0') + 10 * (token[1] - '0') + token[2] - '0';
390  lat += atof(&(token.c_str()[3])) / 60.0;
392  }
393  else
394  all_fields_ok = false;
395 
396  // E/W:
397  token = lstTokens[4];
398  if (token.empty())
399  all_fields_ok = false;
400  else if (token[0] == 'W')
402 
403  if (lstTokens.size() >= 7)
404  {
405  // Time:
406  token = lstTokens[5];
407  if (token.size() >= 6)
408  {
409  gll.fields.UTCTime.hour =
410  10 * (token[0] - '0') + token[1] - '0';
411  gll.fields.UTCTime.minute =
412  10 * (token[2] - '0') + token[3] - '0';
413  gll.fields.UTCTime.sec = atof(&(token.c_str()[4]));
414  }
415  else
416  all_fields_ok = false;
417 
418  // Valid?
419  token = lstTokens[6];
420  if (token.empty())
421  all_fields_ok = false;
422  else
423  gll.fields.validity_char = token.c_str()[0];
424  }
425 
426  if (all_fields_ok)
427  {
428  out_obs.setMsg(gll);
430  out_obs.timestamp =
431  gll.fields.UTCTime.getAsTimestamp(last_known_date);
432  last_known_time = out_obs.timestamp;
433  out_obs.has_satellite_timestamp = true;
434  }
435  parsed_ok = all_fields_ok;
436  }
437  else if (lstTokens[0] == "$GPVTG" && lstTokens.size() >= 9)
438  {
439  // ---------------------------------------------
440  // GPVTG
441  // ---------------------------------------------
442  bool all_fields_ok = true;
443  std::string token;
444 
445  // Fill out the output structure:
447 
448  vtg.fields.true_track = atof(lstTokens[1].c_str());
449  vtg.fields.magnetic_track = atof(lstTokens[3].c_str());
450  vtg.fields.ground_speed_knots = atof(lstTokens[5].c_str());
451  vtg.fields.ground_speed_kmh = atof(lstTokens[7].c_str());
452 
453  if (lstTokens[2] != "T" || lstTokens[4] != "M" || lstTokens[6] != "N" ||
454  lstTokens[8] != "K")
455  all_fields_ok = false;
456 
457  if (all_fields_ok)
458  {
459  out_obs.setMsg(vtg);
461  out_obs.timestamp = last_known_time;
462  out_obs.has_satellite_timestamp = false;
463  }
464  parsed_ok = all_fields_ok;
465  }
466  else if (lstTokens[0] == "$GPZDA" && lstTokens.size() >= 5)
467  {
468  // ---------------------------------------------
469  // GPZDA
470  // ---------------------------------------------
471  bool all_fields_ok = true;
472  std::string token;
473 
474  // Fill out the output structure:
476  //$--ZDA,hhmmss.ss,xx,xx,xxxx,xx,xx
477  // hhmmss.ss = UTC
478  // xx = Day, 01 to 31
479  // xx = Month, 01 to 12
480  // xxxx = Year
481  // xx = Local zone description, 00 to +/- 13 hours
482  // xx = Local zone minutes description (same sign as hours)
483 
484  // Time:
485  token = lstTokens[1];
486  if (token.size() >= 6)
487  {
488  zda.fields.UTCTime.hour = 10 * (token[0] - '0') + token[1] - '0';
489  zda.fields.UTCTime.minute = 10 * (token[2] - '0') + token[3] - '0';
490  zda.fields.UTCTime.sec = atof(&(token.c_str()[4]));
491  }
492  else
493  all_fields_ok = false;
494 
495  // Day:
496  token = lstTokens[2];
497  if (!token.empty()) zda.fields.date_day = atoi(token.c_str());
498  // Month:
499  token = lstTokens[3];
500  if (!token.empty()) zda.fields.date_month = atoi(token.c_str());
501  // Year:
502  token = lstTokens[4];
503  if (!token.empty()) zda.fields.date_year = atoi(token.c_str());
504 
505  if (all_fields_ok)
506  {
507  out_obs.setMsg(zda);
509  try
510  {
511  out_obs.timestamp = zda.getDateTimeAsTimestamp();
512  last_known_date = zda.getDateAsTimestamp();
513  out_obs.has_satellite_timestamp = true;
514  last_known_time = out_obs.timestamp;
515  }
516  catch (...)
517  {
518  // Invalid date:
519  out_obs.timestamp = out_obs.originalReceivedTimestamp;
520  }
521  }
522  parsed_ok = all_fields_ok;
523  }
524  else
525  {
526  // other commands?
527  }
528 
529  return parsed_ok;
530 }
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:-)
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.
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
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.
GLdouble s
Definition: glext.h:3682
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.
GLsizei const GLchar ** string
Definition: glext.h:4116
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)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019