1 /*
2 *
3 * NMEA library
4 * URL: http://nmea.sourceforge.net
5 * Author: Tim (xtimor@gmail.com)
6 * Licence: http://www.gnu.org/licenses/lgpl.html
7 * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $
8 *
9 */
10
11 /**
12 * \file parse.h
13 * \brief Functions of a low level for analysis of
14 * packages of NMEA stream.
15 *
16 * \code
17 * ...
18 * ptype = nmea_pack_type(
19 * (const char *)parser->buffer + nparsed + 1,
20 * parser->buff_use - nparsed - 1);
21 *
22 * if(0 == (node = malloc(sizeof(nmeaParserNODE))))
23 * goto mem_fail;
24 *
25 * node->pack = 0;
26 *
27 * switch(ptype)
28 * {
29 * case GPGGA:
30 * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
31 * goto mem_fail;
32 * node->packType = GPGGA;
33 * if(!nmea_parse_GPGGA(
34 * (const char *)parser->buffer + nparsed,
35 * sen_sz, (nmeaGPGGA *)node->pack))
36 * {
37 * free(node);
38 * node = 0;
39 * }
40 * break;
41 * case GPGSA:
42 * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
43 * goto mem_fail;
44 * node->packType = GPGSA;
45 * if(!nmea_parse_GPGSA(
46 * (const char *)parser->buffer + nparsed,
47 * sen_sz, (nmeaGPGSA *)node->pack))
48 * {
49 * free(node);
50 * node = 0;
51 * }
52 * break;
53 * ...
54 * \endcode
55 */
56
57 #include "nmea/tok.h"
58 #include "nmea/parse.h"
59 #include "nmea/context.h"
60 #include "nmea/gmath.h"
61 #include "nmea/units.h"
62
63 #include <string.h>
64 #include <stdio.h>
65
_nmea_parse_time(const char * buff,int buff_sz,nmeaTIME * res)66 int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res)
67 {
68 int success = 0;
69
70 switch(buff_sz)
71 {
72 case sizeof("hhmmss") - 1:
73 success = (3 == nmea_scanf(buff, buff_sz,
74 "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec)
75 ));
76 break;
77 case sizeof("hhmmss.s") - 1:
78 case sizeof("hhmmss.ss") - 1:
79 case sizeof("hhmmss.sss") - 1:
80 success = (4 == nmea_scanf(buff, buff_sz,
81 "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec)
82 ));
83 break;
84 default:
85 nmea_error("Parse of time error (format error)!");
86 success = 0;
87 break;
88 }
89
90 return (success?0:-1);
91 }
92
93 /**
94 * \brief Define packet type by header (nmeaPACKTYPE).
95 * @param buff a constant character pointer of packet buffer.
96 * @param buff_sz buffer size.
97 * @return The defined packet type
98 * @see nmeaPACKTYPE
99 */
nmea_pack_type(const char * buff,int buff_sz)100 int nmea_pack_type(const char *buff, int buff_sz)
101 {
102 static const char *pheads[] = {
103 "GPGGA",
104 "GPGSA",
105 "GPGSV",
106 "GPRMC",
107 "GPVTG",
108 };
109
110 NMEA_ASSERT(buff);
111
112 if(buff_sz < 5)
113 return GPNON;
114 else if(0 == memcmp(buff, pheads[0], 5))
115 return GPGGA;
116 else if(0 == memcmp(buff, pheads[1], 5))
117 return GPGSA;
118 else if(0 == memcmp(buff, pheads[2], 5))
119 return GPGSV;
120 else if(0 == memcmp(buff, pheads[3], 5))
121 return GPRMC;
122 else if(0 == memcmp(buff, pheads[4], 5))
123 return GPVTG;
124
125 return GPNON;
126 }
127
128 /**
129 * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC).
130 * @param buff a constant character pointer of packets buffer.
131 * @param buff_sz buffer size.
132 * @param res_crc a integer pointer for return CRC of packet (must be defined).
133 * @return Number of bytes to packet tail.
134 */
nmea_find_tail(const char * buff,int buff_sz,int * res_crc)135 int nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
136 {
137 static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
138
139 const char *end_buff = buff + buff_sz;
140 int nread = 0;
141 int crc = 0;
142
143 NMEA_ASSERT(buff && res_crc);
144
145 *res_crc = -1;
146
147 for(;buff < end_buff; ++buff, ++nread)
148 {
149 if(('$' == *buff) && nread)
150 {
151 buff = 0;
152 break;
153 }
154 else if('*' == *buff)
155 {
156 if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4])
157 {
158 *res_crc = nmea_atoi(buff + 1, 2, 16);
159 nread = buff_sz - (int)(end_buff - (buff + tail_sz));
160 if(*res_crc != crc)
161 {
162 *res_crc = -1;
163 buff = 0;
164 }
165 }
166
167 break;
168 }
169 else if(nread)
170 crc ^= (int)*buff;
171 }
172
173 if(*res_crc < 0 && buff)
174 nread = 0;
175
176 return nread;
177 }
178
179 /**
180 * \brief Parse GGA packet from buffer.
181 * @param buff a constant character pointer of packet buffer.
182 * @param buff_sz buffer size.
183 * @param pack a pointer of packet which will filled by function.
184 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
185 */
nmea_parse_GPGGA(const char * buff,int buff_sz,nmeaGPGGA * pack)186 int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
187 {
188 char time_buff[NMEA_TIMEPARSE_BUF];
189
190 NMEA_ASSERT(buff && pack);
191
192 memset(pack, 0, sizeof(nmeaGPGGA));
193
194 nmea_trace_buff(buff, buff_sz);
195
196 if(14 != nmea_scanf(buff, buff_sz,
197 "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
198 &(time_buff[0]),
199 &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
200 &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units),
201 &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid)))
202 {
203 nmea_error("GPGGA parse error!");
204 return 0;
205 }
206
207 if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
208 {
209 nmea_error("GPGGA time parse error!");
210 return 0;
211 }
212
213 return 1;
214 }
215
216 /**
217 * \brief Parse GSA packet from buffer.
218 * @param buff a constant character pointer of packet buffer.
219 * @param buff_sz buffer size.
220 * @param pack a pointer of packet which will filled by function.
221 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
222 */
nmea_parse_GPGSA(const char * buff,int buff_sz,nmeaGPGSA * pack)223 int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack)
224 {
225 NMEA_ASSERT(buff && pack);
226
227 memset(pack, 0, sizeof(nmeaGPGSA));
228
229 nmea_trace_buff(buff, buff_sz);
230
231 if(17 != nmea_scanf(buff, buff_sz,
232 "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*",
233 &(pack->fix_mode), &(pack->fix_type),
234 &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]),
235 &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]),
236 &(pack->PDOP), &(pack->HDOP), &(pack->VDOP)))
237 {
238 nmea_error("GPGSA parse error!");
239 return 0;
240 }
241
242 return 1;
243 }
244
245 /**
246 * \brief Parse GSV packet from buffer.
247 * @param buff a constant character pointer of packet buffer.
248 * @param buff_sz buffer size.
249 * @param pack a pointer of packet which will filled by function.
250 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
251 */
nmea_parse_GPGSV(const char * buff,int buff_sz,nmeaGPGSV * pack)252 int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
253 {
254 int nsen, nsat;
255
256 NMEA_ASSERT(buff && pack);
257
258 memset(pack, 0, sizeof(nmeaGPGSV));
259
260 nmea_trace_buff(buff, buff_sz);
261
262 nsen = nmea_scanf(buff, buff_sz,
263 "$GPGSV,%d,%d,%d,"
264 "%d,%d,%d,%d,"
265 "%d,%d,%d,%d,"
266 "%d,%d,%d,%d,"
267 "%d,%d,%d,%d*",
268 &(pack->pack_count), &(pack->pack_index), &(pack->sat_count),
269 &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig),
270 &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig),
271 &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig),
272 &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig));
273
274 nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
275 nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
276 nsat = nsat * 4 + 3 /* first three sentence`s */;
277
278 if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3))
279 {
280 nmea_error("GPGSV parse error!");
281 return 0;
282 }
283
284 return 1;
285 }
286
287 /**
288 * \brief Parse RMC packet from buffer.
289 * @param buff a constant character pointer of packet buffer.
290 * @param buff_sz buffer size.
291 * @param pack a pointer of packet which will filled by function.
292 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
293 */
nmea_parse_GPRMC(const char * buff,int buff_sz,nmeaGPRMC * pack)294 int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack)
295 {
296 int nsen;
297 char time_buff[NMEA_TIMEPARSE_BUF];
298
299 NMEA_ASSERT(buff && pack);
300
301 memset(pack, 0, sizeof(nmeaGPRMC));
302
303 nmea_trace_buff(buff, buff_sz);
304
305 nsen = nmea_scanf(buff, buff_sz,
306 "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
307 &(time_buff[0]),
308 &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
309 &(pack->speed), &(pack->direction),
310 &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
311 &(pack->declination), &(pack->declin_ew), &(pack->mode));
312
313 if(nsen != 13 && nsen != 14)
314 {
315 nmea_error("GPRMC parse error!");
316 return 0;
317 }
318
319 if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
320 {
321 nmea_error("GPRMC time parse error!");
322 return 0;
323 }
324
325 if(pack->utc.year < 90)
326 pack->utc.year += 100;
327 pack->utc.mon -= 1;
328
329 return 1;
330 }
331
332 /**
333 * \brief Parse VTG packet from buffer.
334 * @param buff a constant character pointer of packet buffer.
335 * @param buff_sz buffer size.
336 * @param pack a pointer of packet which will filled by function.
337 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
338 */
nmea_parse_GPVTG(const char * buff,int buff_sz,nmeaGPVTG * pack)339 int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
340 {
341 NMEA_ASSERT(buff && pack);
342
343 memset(pack, 0, sizeof(nmeaGPVTG));
344
345 nmea_trace_buff(buff, buff_sz);
346
347 if(8 != nmea_scanf(buff, buff_sz,
348 "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
349 &(pack->dir), &(pack->dir_t),
350 &(pack->dec), &(pack->dec_m),
351 &(pack->spn), &(pack->spn_n),
352 &(pack->spk), &(pack->spk_k)))
353 {
354 nmea_error("GPVTG parse error!");
355 return 0;
356 }
357
358 if( pack->dir_t != 'T' ||
359 pack->dec_m != 'M' ||
360 pack->spn_n != 'N' ||
361 pack->spk_k != 'K')
362 {
363 nmea_error("GPVTG parse error (format error)!");
364 return 0;
365 }
366
367 return 1;
368 }
369
370 /**
371 * \brief Fill nmeaINFO structure by GGA packet data.
372 * @param pack a pointer of packet structure.
373 * @param info a pointer of summary information structure.
374 */
nmea_GPGGA2info(nmeaGPGGA * pack,nmeaINFO * info)375 void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info)
376 {
377 NMEA_ASSERT(pack && info);
378
379 info->utc.hour = pack->utc.hour;
380 info->utc.min = pack->utc.min;
381 info->utc.sec = pack->utc.sec;
382 info->utc.hsec = pack->utc.hsec;
383 info->sig = pack->sig;
384 info->HDOP = pack->HDOP;
385 info->elv = pack->elv;
386 info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
387 info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
388 info->smask |= GPGGA;
389 }
390
391 /**
392 * \brief Fill nmeaINFO structure by GSA packet data.
393 * @param pack a pointer of packet structure.
394 * @param info a pointer of summary information structure.
395 */
nmea_GPGSA2info(nmeaGPGSA * pack,nmeaINFO * info)396 void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
397 {
398 int i, j, nuse = 0;
399
400 NMEA_ASSERT(pack && info);
401
402 info->fix = pack->fix_type;
403 info->PDOP = pack->PDOP;
404 info->HDOP = pack->HDOP;
405 info->VDOP = pack->VDOP;
406
407 for(i = 0; i < NMEA_MAXSAT; ++i)
408 {
409 for(j = 0; j < info->satinfo.inview; ++j)
410 {
411 if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id)
412 {
413 info->satinfo.sat[j].in_use = 1;
414 nuse++;
415 }
416 }
417 }
418
419 info->satinfo.inuse = nuse;
420 info->smask |= GPGSA;
421 }
422
423 /**
424 * \brief Fill nmeaINFO structure by GSV packet data.
425 * @param pack a pointer of packet structure.
426 * @param info a pointer of summary information structure.
427 */
nmea_GPGSV2info(nmeaGPGSV * pack,nmeaINFO * info)428 void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
429 {
430 int isat, isi, nsat;
431
432 NMEA_ASSERT(pack && info);
433
434 if(pack->pack_index > pack->pack_count ||
435 pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT)
436 return;
437
438 if(pack->pack_index < 1)
439 pack->pack_index = 1;
440
441 info->satinfo.inview = pack->sat_count;
442
443 nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
444 nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
445
446 for(isat = 0; isat < nsat; ++isat)
447 {
448 isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat;
449 info->satinfo.sat[isi].id = pack->sat_data[isat].id;
450 info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
451 info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
452 info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
453 }
454
455 info->smask |= GPGSV;
456 }
457
458 /**
459 * \brief Fill nmeaINFO structure by RMC packet data.
460 * @param pack a pointer of packet structure.
461 * @param info a pointer of summary information structure.
462 */
nmea_GPRMC2info(nmeaGPRMC * pack,nmeaINFO * info)463 void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info)
464 {
465 NMEA_ASSERT(pack && info);
466
467 if('A' == pack->status)
468 {
469 if(NMEA_SIG_BAD == info->sig)
470 info->sig = NMEA_SIG_MID;
471 if(NMEA_FIX_BAD == info->fix)
472 info->fix = NMEA_FIX_2D;
473 }
474 else if('V' == pack->status)
475 {
476 info->sig = NMEA_SIG_BAD;
477 info->fix = NMEA_FIX_BAD;
478 }
479
480 info->utc = pack->utc;
481 info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
482 info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
483 info->speed = pack->speed * NMEA_TUD_KNOTS;
484 info->direction = pack->direction;
485 info->smask |= GPRMC;
486 }
487
488 /**
489 * \brief Fill nmeaINFO structure by VTG packet data.
490 * @param pack a pointer of packet structure.
491 * @param info a pointer of summary information structure.
492 */
nmea_GPVTG2info(nmeaGPVTG * pack,nmeaINFO * info)493 void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
494 {
495 NMEA_ASSERT(pack && info);
496
497 info->direction = pack->dir;
498 info->declination = pack->dec;
499 info->speed = pack->spk;
500 info->smask |= GPVTG;
501 }
502