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