Main MRPT website > C++ reference for MRPT 1.5.6
CGPSInterface_parser_NMEA.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/os.h>
14 #include <mrpt/system/filesystem.h>
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)
29  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 peek_buffer[3];
35  m_rx_buffer.peek_many(&peek_buffer[0],3);
36  if (peek_buffer[0]!='$' || peek_buffer[1]!='G' || peek_buffer[2]!='P') {
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  line_is_ended = true;
50  break;
51  }
52  line.push_back(val);
53  }
54  if (line_is_ended)
55  {
56  // Pop from buffer:
57  for (size_t i=0;i<line.size();i++) m_rx_buffer.pop();
58 
59  // Parse:
60  const bool did_have_gga = m_just_parsed_messages.has_GGA_datum;
61  if (CGPSInterface::parse_NMEA(line,m_just_parsed_messages, false /*verbose*/))
62  {
63  // Parsers must set only the part of the msg type:
64  m_just_parsed_messages.sensorLabel = "NMEA";
65 
66  // Save GGA cache (useful for NTRIP,...)
67  const bool now_has_gga = m_just_parsed_messages.has_GGA_datum;
68  if (now_has_gga && !did_have_gga) {
69  m_last_GGA = line;
70  }
71  }
72  else
73  {
74  if (m_verbose)
75  std::cerr << "[CGPSInterface::implement_parser_NMEA] Line of unknown format ignored: `"<<line<<"`\n";
76  }
77  return true;
78  }
79  else
80  {
81  // We still need to wait for more data to be read:
82  out_minimum_rx_buf_to_decide = nBytesAval+1;
83  return true;
84  }
85  }
86 }
87 
88 /* -----------------------------------------------------
89  parse_NMEA
90 ----------------------------------------------------- */
91 bool CGPSInterface::parse_NMEA(const std::string &s, mrpt::obs::CObservationGPS &out_obs, const bool verbose)
92 {
93  static mrpt::system::TTimeStamp last_known_date = mrpt::system::now(); // For building complete date+time in msgs without a date.
94  static mrpt::system::TTimeStamp last_known_time = mrpt::system::now();
95 
96  if (verbose)
97  cout << "[CGPSInterface] GPS raw string: " << s << endl;
98 
99  // Firstly! If the string does not start with "$GP" it is not valid:
100  if (s.size()<7) return false;
101  if ( s[0]!='$' || s[1]!='G' ) return false;
102 
103  std::vector<std::string> lstTokens;
104  mrpt::system::tokenize(s,"*,\t\r\n",lstTokens, false /* do not skip blank tokens */);
105  if (lstTokens.size()<3) return false;
106 
107  for (size_t i=0;i<lstTokens.size();i++) lstTokens[i] = mrpt::system::trim(lstTokens[i]); // Trim whitespaces
108 
109  bool parsed_ok = false;
110  // Try to determine the kind of command:
111  if (lstTokens[0]=="$GPGGA" && lstTokens.size()>=13)
112  {
113  // ---------------------------------------------
114  // GGA
115  // ---------------------------------------------
116  bool all_fields_ok=true;
117  std::string token;
118 
119  // Fill out the output structure:
121 
122  // Time:
123  token = lstTokens[1];
124  if (token.size()>=6)
125  {
126  gga.fields.UTCTime.hour = 10 * (token[0]-'0') + token[1]-'0';
127  gga.fields.UTCTime.minute = 10 * (token[2]-'0') + token[3]-'0';
128  gga.fields.UTCTime.sec = atof( & (token.c_str()[4]) );
129  }
130  else all_fields_ok = false;
131 
132  // Latitude:
133  token = lstTokens[2];
134  if (token.size()>=4)
135  {
136  double lat = 10 * (token[0]-'0') + token[1]-'0';
137  lat += atof( & (token.c_str()[2]) ) / 60.0;
139  }
140  else all_fields_ok = false;
141 
142  // N/S:
143  token = lstTokens[3];
144  if (token.empty())
145  all_fields_ok = false;
146  else if (token[0]=='S')
148 
149  // Longitude:
150  token = lstTokens[4];
151  if (token.size()>=5)
152  {
153  double lat = 100 * (token[0]-'0') + 10 * (token[1]-'0')+ token[2]-'0';
154  lat += atof( & (token.c_str()[3]) ) / 60.0;
156  }
157  else all_fields_ok = false;
158 
159  // E_W:
160  token = lstTokens[5];
161  if (token.empty())
162  all_fields_ok = false;
163  else
164  if (token[0]=='W')
166 
167  // fix quality:
168  token = lstTokens[6];
169  if (!token.empty())
170  gga.fields.fix_quality = (unsigned char)atoi(token.c_str());
171 
172  // sats:
173  token = lstTokens[7];
174  if (!token.empty())
175  gga.fields.satellitesUsed = (unsigned char)atoi( token.c_str() );
176 
177  // HDOP:
178  token = lstTokens[8];
179  if (!token.empty())
180  {
181  gga.fields.HDOP = (float)atof( token.c_str() );
182  gga.fields.thereis_HDOP = true;
183  }
184 
185  // Altitude:
186  token = lstTokens[9];
187  if (token.empty()) all_fields_ok = false;
188  else gga.fields.altitude_meters = atof( token.c_str() );
189 
190  // Units of the altitude:
191 // token = lstTokens[10];
192 // ASSERT_(token == "M");
193 
194  // Geoidal separation [B] (undulation)
195  token = lstTokens[11];
196  if (!token.empty())
197  gga.fields.geoidal_distance = atof( token.c_str() );
198 
199  // Units of the geoidal separation:
200 // token = lstTokens[12];
201 // ASSERT_(token == "M");
202 
203  // Total altitude [A]+[B] and mmGPS Corrected total altitude Corr([A]+[B]):
207 
208  if (all_fields_ok) {
209  out_obs.setMsg(gga);
211  out_obs.timestamp = gga.fields.UTCTime.getAsTimestamp( last_known_date );
212  out_obs.has_satellite_timestamp = true;
213  }
214  parsed_ok = all_fields_ok;
215  }
216  else if ( lstTokens[0]=="$GPRMC" && lstTokens.size()>=13)
217  {
218  // ---------------------------------------------
219  // GPRMC
220  // ---------------------------------------------
221  bool all_fields_ok = true;
222  std::string token;
223 
224  // Fill out the output structure:
226 
227  // Time:
228  token = lstTokens[1];
229  if (token.size()>=6)
230  {
231  rmc.fields.UTCTime.hour = 10 * (token[0]-'0') + token[1]-'0';
232  rmc.fields.UTCTime.minute = 10 * (token[2]-'0') + token[3]-'0';
233  rmc.fields.UTCTime.sec = atof( & (token.c_str()[4]) );
234  }
235  else all_fields_ok = false;
236 
237  // Valid?
238  token = lstTokens[2];
239  if (token.empty()) all_fields_ok = false;
240  else rmc.fields.validity_char = token.c_str()[0];
241 
242  // Latitude:
243  token = lstTokens[3];
244  if (token.size()>=4)
245  {
246  double lat = 10 * (token[0]-'0') + token[1]-'0';
247  lat += atof( & (token.c_str()[2]) ) / 60.0;
249  }
250  else all_fields_ok = false;
251 
252  // N/S:
253  token = lstTokens[4];
254  if (token.empty()) all_fields_ok = false;
255  else if (token[0]=='S')
257 
258  // Longitude:
259  token = lstTokens[5];
260  if (token.size()>=5)
261  {
262  double lat = 100 * (token[0]-'0') + 10 * (token[1]-'0')+ token[2]-'0';
263  lat += atof( & (token.c_str()[3]) ) / 60.0;
265  }
266  else all_fields_ok = false;
267 
268  // E/W:
269  token = lstTokens[6];
270  if (token.empty()) all_fields_ok = false;
271  else if (token[0]=='W')
273 
274  // Speed:
275  token = lstTokens[7];
276  if (!token.empty()) rmc.fields.speed_knots = atof( token.c_str() );
277 
278  // Direction:
279  token = lstTokens[8];
280  if (!token.empty()) rmc.fields.direction_degrees= atof( token.c_str() );
281 
282  // Date:
283  token = lstTokens[9];
284  if (token.size()>=6) {
285  rmc.fields.date_day = 10 * (token[0]-'0') + token[1]-'0';
286  rmc.fields.date_month = 10 * (token[2]-'0') + token[3]-'0';
287  rmc.fields.date_year = atoi( & (token.c_str()[4]) );
288  }
289  else all_fields_ok = false;
290 
291  // Magnetic var
292  token = lstTokens[10];
293  if (token.size()>=2)
294  {
295  rmc.fields.magnetic_dir = atof(token.c_str());
296  // E/W:
297  token = lstTokens[11];
298  if (token.empty()) all_fields_ok = false;
299  else if (token[0]=='W')
301  }
302 
303  // Mode ind.
304  if (lstTokens.size()>=14) {
305  // Only for NMEA 2.3
306  token = lstTokens[12];
307  if (token.empty()) all_fields_ok = false;
308  else rmc.fields.positioning_mode = token.c_str()[0];
309  } else rmc.fields.positioning_mode = 'A'; // Default for older receiver
310 
311  if (all_fields_ok) {
312  out_obs.setMsg(rmc);
315  last_known_date = rmc.getDateAsTimestamp();
316  last_known_time = out_obs.timestamp;
317  out_obs.has_satellite_timestamp = true;
318  }
319  parsed_ok = all_fields_ok;
320  }
321  else if ( lstTokens[0]=="$GPGLL" && lstTokens.size()>=5)
322  {
323  // ---------------------------------------------
324  // GPGLL
325  // ---------------------------------------------
326  bool all_fields_ok = true;
327  std::string token;
328 
329  // Fill out the output structure:
331  // Latitude:
332  token = lstTokens[1];
333  if (token.size()>=4)
334  {
335  double lat = 10 * (token[0]-'0') + token[1]-'0';
336  lat += atof( & (token.c_str()[2]) ) / 60.0;
338  }
339  else all_fields_ok = false;
340 
341  // N/S:
342  token = lstTokens[2];
343  if (token.empty()) all_fields_ok = false;
344  else if (token[0]=='S')
346 
347  // Longitude:
348  token = lstTokens[3];
349  if (token.size()>=5)
350  {
351  double lat = 100 * (token[0]-'0') + 10 * (token[1]-'0')+ token[2]-'0';
352  lat += atof( & (token.c_str()[3]) ) / 60.0;
354  }
355  else all_fields_ok = false;
356 
357  // E/W:
358  token = lstTokens[4];
359  if (token.empty()) all_fields_ok = false;
360  else if (token[0]=='W')
362 
363  if (lstTokens.size()>=7) {
364  // Time:
365  token = lstTokens[5];
366  if (token.size()>=6)
367  {
368  gll.fields.UTCTime.hour = 10 * (token[0]-'0') + token[1]-'0';
369  gll.fields.UTCTime.minute = 10 * (token[2]-'0') + token[3]-'0';
370  gll.fields.UTCTime.sec = atof( & (token.c_str()[4]) );
371  }
372  else all_fields_ok = false;
373 
374  // Valid?
375  token = lstTokens[6];
376  if (token.empty()) all_fields_ok = false;
377  else gll.fields.validity_char = token.c_str()[0];
378  }
379 
380  if (all_fields_ok) {
381  out_obs.setMsg(gll);
383  out_obs.timestamp = gll.fields.UTCTime.getAsTimestamp( last_known_date );
384  last_known_time = out_obs.timestamp;
385  out_obs.has_satellite_timestamp = true;
386  }
387  parsed_ok = all_fields_ok;
388  }
389  else if ( lstTokens[0]=="$GPVTG" && lstTokens.size()>=9)
390  {
391  // ---------------------------------------------
392  // GPVTG
393  // ---------------------------------------------
394  bool all_fields_ok = true;
395  std::string token;
396 
397  // Fill out the output structure:
399 
400  vtg.fields.true_track = atof(lstTokens[1].c_str());
401  vtg.fields.magnetic_track = atof(lstTokens[3].c_str());
402  vtg.fields.ground_speed_knots = atof(lstTokens[5].c_str());
403  vtg.fields.ground_speed_kmh = atof(lstTokens[7].c_str());
404 
405  if (lstTokens[2]!="T" ||lstTokens[4]!="M" ||lstTokens[6]!="N" ||lstTokens[8]!="K")
406  all_fields_ok=false;
407 
408  if (all_fields_ok) {
409  out_obs.setMsg(vtg);
411  out_obs.timestamp = last_known_time;
412  out_obs.has_satellite_timestamp = false;
413  }
414  parsed_ok = all_fields_ok;
415 
416  }
417  else if ( lstTokens[0]=="$GPZDA" && lstTokens.size()>=5)
418  {
419  // ---------------------------------------------
420  // GPZDA
421  // ---------------------------------------------
422  bool all_fields_ok = true;
423  std::string token;
424 
425  // Fill out the output structure:
427  //$--ZDA,hhmmss.ss,xx,xx,xxxx,xx,xx
428  //hhmmss.ss = UTC
429  //xx = Day, 01 to 31
430  //xx = Month, 01 to 12
431  //xxxx = Year
432  //xx = Local zone description, 00 to +/- 13 hours
433  //xx = Local zone minutes description (same sign as hours)
434 
435  // Time:
436  token = lstTokens[1];
437  if (token.size()>=6) {
438  zda.fields.UTCTime.hour = 10 * (token[0]-'0') + token[1]-'0';
439  zda.fields.UTCTime.minute = 10 * (token[2]-'0') + token[3]-'0';
440  zda.fields.UTCTime.sec = atof( & (token.c_str()[4]) );
441  }
442  else all_fields_ok = false;
443 
444  // Day:
445  token = lstTokens[2];
446  if (!token.empty())
447  zda.fields.date_day = atoi(token.c_str());
448  // Month:
449  token = lstTokens[3];
450  if (!token.empty())
451  zda.fields.date_month = atoi(token.c_str());
452  // Year:
453  token = lstTokens[4];
454  if (!token.empty())
455  zda.fields.date_year = atoi(token.c_str());
456 
457  if (all_fields_ok) {
458  out_obs.setMsg(zda);
460  try {
461  out_obs.timestamp = zda.getDateTimeAsTimestamp();
462  last_known_date = zda.getDateAsTimestamp();
463  out_obs.has_satellite_timestamp = true;
464  last_known_time = out_obs.timestamp;
465  } catch (...) {
466  // Invalid date:
467  out_obs.timestamp
468  = out_obs.originalReceivedTimestamp;
469  }
470  }
471  parsed_ok = all_fields_ok;
472  }
473  else
474  {
475  // other commands?
476  }
477 
478  return parsed_ok;
479 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
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:-)
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:70
content_t fields
Message content, accesible by individual fields.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
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...
int8_t validity_char
This will be: 'A'=OK or 'V'=void.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
double orthometric_altitude
The measured orthometric altitude, in meters (A)+(B).
unsigned char uint8_t
Definition: rptypes.h:43
double altitude_meters
The measured altitude, in meters (A).
GLdouble s
Definition: glew.h:1295
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). ...
bool implement_parser_NMEA(size_t &out_minimum_rx_buf_to_decide)
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:-)
bool has_satellite_timestamp
If true, CObservation::timestamp has been generated from accurate satellite clock. Otherwise, no GPS data is available and timestamps are based on the local computer clock.
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
uint8_t date_year
Date: day (1-31), month (1-12), two-digits year (00-99)
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...
mrpt::system::TTimeStamp getDateTimeAsTimestamp() const
Build an MRPT UTC timestamp with the year/month/day + hour/minute/sec of this observation.
double geoidal_distance
Undulation: Difference between the measured altitude and the geoid, in meters (B).
double magnetic_dir
Magnetic variation direction (East:+, West:-)
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. Where available, this should contain the accurate satellite-based time...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
double direction_degrees
Measured speed direction (in degrees)
char positioning_mode
'A': Autonomous, 'D': Differential, 'N': Not valid, 'E': Estimated, 'M': Manual
content_t fields
Message content, accesible by individual fields.
std::string BASE_IMPEXP 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: 'A'=OK or 'V'=void.
GLuint GLfloat * val
Definition: glew.h:7785
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018