Branch data Line data Source code
1 : : /*
2 : : * Copyright Tim (xtimor@gmail.com)
3 : : *
4 : : * NMEA library is free software; you can redistribute it and/or modify
5 : : * it under the terms of the GNU Lesser General Public License as published by
6 : : * the Free Software Foundation; either version 2 of the License, or
7 : : * (at your option) any later version.
8 : : *
9 : : * This program is distributed in the hope that it will be useful,
10 : : * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : : * GNU Lesser General Public License for more details.
13 : : *
14 : : * You should have received a copy of the GNU Lesser General Public License
15 : : * along with this program. If not, see <http://www.gnu.org/licenses/>
16 : : */
17 : : /*
18 : : *
19 : : * NMEA library
20 : : * URL: http://nmea.sourceforge.net
21 : : * Author: Tim (xtimor@gmail.com)
22 : : * Licence: http://www.gnu.org/licenses/lgpl.html
23 : : * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $
24 : : *
25 : : */
26 : :
27 : : /**
28 : : * \file parse.h
29 : : * \brief Functions of a low level for analysis of
30 : : * packages of NMEA stream.
31 : : *
32 : : * \code
33 : : * ...
34 : : * ptype = nmea_pack_type(
35 : : * (const char *)parser->buffer + nparsed + 1,
36 : : * parser->buff_use - nparsed - 1);
37 : : *
38 : : * if(0 == (node = malloc(sizeof(nmeaParserNODE))))
39 : : * goto mem_fail;
40 : : *
41 : : * node->pack = 0;
42 : : *
43 : : * switch(ptype)
44 : : * {
45 : : * case GPGGA:
46 : : * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
47 : : * goto mem_fail;
48 : : * node->packType = GPGGA;
49 : : * if(!nmea_parse_GPGGA(
50 : : * (const char *)parser->buffer + nparsed,
51 : : * sen_sz, (nmeaGPGGA *)node->pack))
52 : : * {
53 : : * free(node);
54 : : * node = 0;
55 : : * }
56 : : * break;
57 : : * case GPGSA:
58 : : * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
59 : : * goto mem_fail;
60 : : * node->packType = GPGSA;
61 : : * if(!nmea_parse_GPGSA(
62 : : * (const char *)parser->buffer + nparsed,
63 : : * sen_sz, (nmeaGPGSA *)node->pack))
64 : : * {
65 : : * free(node);
66 : : * node = 0;
67 : : * }
68 : : * break;
69 : : * ...
70 : : * \endcode
71 : : */
72 : :
73 : : #include "tok.h"
74 : : #include "parse.h"
75 : : #include "context.h"
76 : : #include "gmath.h"
77 : : #include "units.h"
78 : :
79 : : #include <string.h>
80 : : #include <stdio.h>
81 : :
82 : 0 : int _nmea_parse_time( const char *buff, int buff_sz, nmeaTIME *res )
83 : : {
84 : 0 : int success = 0;
85 : :
86 : 0 : switch ( buff_sz )
87 : : {
88 : : case sizeof( "hhmmss" ) - 1:
89 : 0 : success = ( 3 == nmea_scanf( buff, buff_sz,
90 : 0 : "%2d%2d%2d", &( res->hour ), &( res->min ), &( res->sec )
91 : : ) );
92 : 0 : break;
93 : : case sizeof( "hhmmss.s" ) - 1:
94 : 0 : success = ( 4 == nmea_scanf( buff, buff_sz,
95 : 0 : "%2d%2d%2d.%d", &( res->hour ), &( res->min ), &( res->sec ), &( res->msec )
96 : : ) );
97 : 0 : res->msec = res->msec * 100; // tenths sec * 100 = thousandths
98 : 0 : break;
99 : : case sizeof( "hhmmss.ss" ) - 1:
100 : 0 : success = ( 4 == nmea_scanf( buff, buff_sz,
101 : 0 : "%2d%2d%2d.%d", &( res->hour ), &( res->min ), &( res->sec ), &( res->msec )
102 : : ) );
103 : 0 : res->msec = res->msec * 10; // hundredths sec * 10 = thousandths
104 : 0 : break;
105 : : case sizeof( "hhmmss.sss" ) - 1:
106 : 0 : success = ( 4 == nmea_scanf( buff, buff_sz,
107 : 0 : "%2d%2d%2d.%d", &( res->hour ), &( res->min ), &( res->sec ), &( res->msec )
108 : : ) );
109 : : // already thousandths
110 : 0 : break;
111 : : default:
112 : 0 : nmea_error( "Parse of time error (format error)!" );
113 : 0 : success = 0;
114 : 0 : break;
115 : : }
116 : :
117 : 0 : return ( success ? 0 : -1 );
118 : : }
119 : :
120 : : /**
121 : : * \brief Define packet type by header (nmeaPACKTYPE).
122 : : * @param buff a constant character pointer of packet buffer.
123 : : * @param buff_sz buffer size.
124 : : * @return The defined packet type
125 : : * @see nmeaPACKTYPE
126 : : */
127 : 0 : int nmea_pack_type( const char *buff, int buff_sz )
128 : : {
129 : : static const char *P_HEADS[] =
130 : : {
131 : : "GPGGA",
132 : : "GPGSA",
133 : : "GPGSV",
134 : : "GPRMC",
135 : : "GPVTG",
136 : : "HCHDG",
137 : : "HCHDT",
138 : : "GNRMC",
139 : : "GPGST",
140 : : };
141 : :
142 : : // BUFFER_SIZE = size(P_HEADS) - 1;
143 : 0 : int buffer_size = 8;
144 : :
145 : 0 : NMEA_ASSERT( buff );
146 : :
147 : 0 : if ( buff_sz < buffer_size )
148 : 0 : return GPNON;
149 : 0 : else if ( 0 == memcmp( buff, P_HEADS[0], buffer_size ) )
150 : 0 : return GPGGA;
151 : 0 : else if ( 0 == memcmp( buff, P_HEADS[1], buffer_size ) )
152 : 0 : return GPGSA;
153 : 0 : else if ( 0 == memcmp( buff, P_HEADS[2], buffer_size ) )
154 : 0 : return GPGSV;
155 : 0 : else if ( 0 == memcmp( buff, P_HEADS[3], buffer_size ) )
156 : 0 : return GPRMC;
157 : 0 : else if ( 0 == memcmp( buff, P_HEADS[4], buffer_size ) )
158 : 0 : return GPVTG;
159 : 0 : else if ( 0 == memcmp( buff, P_HEADS[5], buffer_size ) )
160 : 0 : return HCHDG;
161 : 0 : else if ( 0 == memcmp( buff, P_HEADS[6], buffer_size ) )
162 : 0 : return HCHDT;
163 : 0 : else if ( 0 == memcmp( buff, P_HEADS[7], buffer_size ) )
164 : 0 : return GPRMC;
165 : 0 : else if ( 0 == memcmp( buff, P_HEADS[8], buffer_size ) )
166 : 0 : return GPGST;
167 : :
168 : 0 : return GPNON;
169 : 0 : }
170 : :
171 : : /**
172 : : * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC).
173 : : * @param buff a constant character pointer of packets buffer.
174 : : * @param buff_sz buffer size.
175 : : * @param res_crc a integer pointer for return CRC of packet (must be defined).
176 : : * @return Number of bytes to packet tail.
177 : : */
178 : 0 : int nmea_find_tail( const char *buff, int buff_sz, int *res_crc )
179 : : {
180 : : static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
181 : :
182 : 0 : const char *end_buff = buff + buff_sz;
183 : 0 : int nread = 0;
184 : 0 : int crc = 0;
185 : :
186 : 0 : NMEA_ASSERT( buff && res_crc );
187 : :
188 : 0 : *res_crc = -1;
189 : :
190 : 0 : for ( ; buff < end_buff; ++buff, ++nread )
191 : : {
192 : 0 : if ( ( '$' == *buff ) && nread )
193 : : {
194 : 0 : buff = 0;
195 : 0 : break;
196 : : }
197 : 0 : else if ( '*' == *buff )
198 : : {
199 : 0 : if ( buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4] )
200 : : {
201 : 0 : *res_crc = nmea_atoi( buff + 1, 2, 16 );
202 : 0 : nread = buff_sz - ( int )( end_buff - ( buff + tail_sz ) );
203 : 0 : if ( *res_crc != crc )
204 : : {
205 : 0 : *res_crc = -1;
206 : 0 : buff = 0;
207 : 0 : }
208 : 0 : }
209 : :
210 : 0 : break;
211 : : }
212 : 0 : else if ( nread )
213 : 0 : crc ^= ( int ) * buff;
214 : 0 : }
215 : :
216 : 0 : if ( *res_crc < 0 && buff )
217 : 0 : nread = 0;
218 : :
219 : 0 : return nread;
220 : : }
221 : :
222 : :
223 : : /**
224 : : * \brief Parse GGA packet from buffer.
225 : : * @param buff a constant character pointer of packet buffer.
226 : : * @param buff_sz buffer size.
227 : : * @param pack a pointer of packet which will filled by function.
228 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
229 : : */
230 : 0 : int nmea_parse_GPGGA( const char *buff, int buff_sz, nmeaGPGGA *pack )
231 : : {
232 : : char time_buff[NMEA_TIMEPARSE_BUF];
233 : :
234 : 0 : NMEA_ASSERT( buff && pack );
235 : :
236 : 0 : memset( pack, 0, sizeof( nmeaGPGGA ) );
237 : :
238 : 0 : nmea_trace_buff( buff, buff_sz );
239 : :
240 : : char type;
241 : :
242 : 0 : if ( 15 != nmea_scanf( buff, buff_sz,
243 : : "$G%CGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
244 : : &( type ),
245 : 0 : &( time_buff[0] ),
246 : 0 : &( pack->lat ), &( pack->ns ), &( pack->lon ), &( pack->ew ),
247 : 0 : &( pack->sig ), &( pack->satinuse ), &( pack->HDOP ), &( pack->elv ), &( pack->elv_units ),
248 : 0 : &( pack->diff ), &( pack->diff_units ), &( pack->dgps_age ), &( pack->dgps_sid ) ) )
249 : : {
250 : 0 : nmea_error( "G?GGA parse error!" );
251 : 0 : return 0;
252 : : }
253 : :
254 : 0 : if ( type != 'P' && type != 'N' )
255 : : {
256 : 0 : nmea_error( "G?GGA invalid type " );
257 : 0 : return 0;
258 : : }
259 : :
260 : 0 : if ( 0 != _nmea_parse_time( &time_buff[0], ( int )strlen( &time_buff[0] ), &( pack->utc ) ) )
261 : : {
262 : 0 : nmea_error( "GPGGA time parse error!" );
263 : 0 : return 0;
264 : : }
265 : :
266 : 0 : return 1;
267 : 0 : }
268 : :
269 : : /**
270 : : * \brief Parse GST packet from buffer.
271 : : * @param buff a constant character pointer of packet buffer.
272 : : * @param buff_sz buffer size.
273 : : * @param pack a pointer of packet which will filled by function.
274 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
275 : : */
276 : 0 : int nmea_parse_GPGST( const char *buff, int buff_sz, nmeaGPGST *pack )
277 : : {
278 : : char time_buff[NMEA_TIMEPARSE_BUF];
279 : :
280 : 0 : NMEA_ASSERT( buff && pack );
281 : :
282 : 0 : memset( pack, 0, sizeof( nmeaGPGST ) );
283 : :
284 : 0 : nmea_trace_buff( buff, buff_sz );
285 : :
286 : : char type;
287 : :
288 : 0 : if ( 9 != nmea_scanf( buff, buff_sz,
289 : : "$G%CGST,%s,%f,%f,%f,%f,%f,%f,%f*",
290 : : &( type ),
291 : 0 : &( time_buff[0] ),
292 : 0 : &( pack->rms_pr ), &( pack->err_major ), &( pack->err_minor ), &( pack->err_ori ),
293 : 0 : &( pack->sig_lat ), &( pack->sig_lon ), &( pack->sig_alt ) ) )
294 : : {
295 : 0 : nmea_error( "G?GST parse error!" );
296 : 0 : return 0;
297 : : }
298 : :
299 : 0 : if ( type != 'P' && type != 'N' )
300 : : {
301 : 0 : nmea_error( "G?GST invalid type " );
302 : 0 : return 0;
303 : : }
304 : :
305 : 0 : if ( 0 != _nmea_parse_time( &time_buff[0], ( int )strlen( &time_buff[0] ), &( pack->utc ) ) )
306 : : {
307 : 0 : nmea_error( "G?GST time parse error!" );
308 : 0 : return 0;
309 : : }
310 : :
311 : 0 : return 1;
312 : 0 : }
313 : :
314 : : /**
315 : : * \brief Parse GSA packet from buffer.
316 : : * @param buff a constant character pointer of packet buffer.
317 : : * @param buff_sz buffer size.
318 : : * @param pack a pointer of packet which will filled by function.
319 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
320 : : */
321 : 0 : int nmea_parse_GPGSA( const char *buff, int buff_sz, nmeaGPGSA *pack )
322 : : {
323 : 0 : NMEA_ASSERT( buff && pack );
324 : :
325 : 0 : memset( pack, 0, sizeof( nmeaGPGSA ) );
326 : :
327 : 0 : nmea_trace_buff( buff, buff_sz );
328 : :
329 : : char type;
330 : :
331 : 0 : if ( 18 != nmea_scanf( buff, buff_sz,
332 : : "$G%CGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*",
333 : 0 : &( type ), &( pack->fix_mode ), &( pack->fix_type ),
334 : 0 : &( pack->sat_prn[0] ), &( pack->sat_prn[1] ), &( pack->sat_prn[2] ), &( pack->sat_prn[3] ), &( pack->sat_prn[4] ), &( pack->sat_prn[5] ),
335 : 0 : &( pack->sat_prn[6] ), &( pack->sat_prn[7] ), &( pack->sat_prn[8] ), &( pack->sat_prn[9] ), &( pack->sat_prn[10] ), &( pack->sat_prn[11] ),
336 : 0 : &( pack->PDOP ), &( pack->HDOP ), &( pack->VDOP ) ) )
337 : : {
338 : 0 : nmea_error( "G?GSA parse error!" );
339 : 0 : return 0;
340 : : }
341 : :
342 : 0 : if ( type != 'P' && type != 'N' )
343 : : {
344 : 0 : nmea_error( "G?GSA invalid type " );
345 : 0 : return 0;
346 : : }
347 : :
348 : 0 : return 1;
349 : 0 : }
350 : :
351 : : /**
352 : : * \brief Parse GSV packet from buffer.
353 : : * @param buff a constant character pointer of packet buffer.
354 : : * @param buff_sz buffer size.
355 : : * @param pack a pointer of packet which will filled by function.
356 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
357 : : */
358 : 0 : int nmea_parse_GPGSV( const char *buff, int buff_sz, nmeaGPGSV *pack )
359 : : {
360 : : int nsen, nsat;
361 : :
362 : 0 : NMEA_ASSERT( buff && pack );
363 : :
364 : 0 : memset( pack, 0, sizeof( nmeaGPGSV ) );
365 : :
366 : 0 : nmea_trace_buff( buff, buff_sz );
367 : :
368 : : char type;
369 : :
370 : 0 : nsen = nmea_scanf( buff, buff_sz,
371 : : "$G%CGSV,%d,%d,%d,"
372 : : "%d,%d,%d,%d,"
373 : : "%d,%d,%d,%d,"
374 : : "%d,%d,%d,%d,"
375 : : "%d,%d,%d,%d*",
376 : : &( type ),
377 : 0 : &( pack->pack_count ), &( pack->pack_index ), &( pack->sat_count ),
378 : 0 : &( pack->sat_data[0].id ), &( pack->sat_data[0].elv ), &( pack->sat_data[0].azimuth ), &( pack->sat_data[0].sig ),
379 : 0 : &( pack->sat_data[1].id ), &( pack->sat_data[1].elv ), &( pack->sat_data[1].azimuth ), &( pack->sat_data[1].sig ),
380 : 0 : &( pack->sat_data[2].id ), &( pack->sat_data[2].elv ), &( pack->sat_data[2].azimuth ), &( pack->sat_data[2].sig ),
381 : 0 : &( pack->sat_data[3].id ), &( pack->sat_data[3].elv ), &( pack->sat_data[3].azimuth ), &( pack->sat_data[3].sig ) );
382 : :
383 : 0 : nsat = ( pack->pack_index - 1 ) * NMEA_SATINPACK;
384 : 0 : nsat = ( nsat + NMEA_SATINPACK > pack->sat_count ) ? pack->sat_count - nsat : NMEA_SATINPACK;
385 : 0 : nsat = nsat * 4 + 3 /* first three sentence`s */;
386 : :
387 : 0 : if ( nsen - 1 < nsat || nsen - 1 > ( NMEA_SATINPACK * 4 + 3 ) )
388 : : {
389 : 0 : nmea_error( "G?GSV parse error!" );
390 : 0 : return 0;
391 : : }
392 : :
393 : 0 : if ( type != 'P' && type != 'N' )
394 : : {
395 : 0 : nmea_error( "G?GSV invalid type " );
396 : 0 : return 0;
397 : : }
398 : :
399 : 0 : return 1;
400 : 0 : }
401 : :
402 : : /**
403 : : * \brief Parse RMC packet from buffer.
404 : : * @param buff a constant character pointer of packet buffer.
405 : : * @param buff_sz buffer size.
406 : : * @param pack a pointer of packet which will filled by function.
407 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
408 : : */
409 : 0 : int nmea_parse_GPRMC( const char *buff, int buff_sz, nmeaGPRMC *pack )
410 : : {
411 : : int nsen;
412 : : char type;
413 : : char time_buff[NMEA_TIMEPARSE_BUF];
414 : :
415 : 0 : NMEA_ASSERT( buff && pack );
416 : :
417 : 0 : memset( pack, 0, sizeof( nmeaGPRMC ) );
418 : :
419 : 0 : nmea_trace_buff( buff, buff_sz );
420 : :
421 : 0 : nsen = nmea_scanf( buff, buff_sz,
422 : : "$G%CRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
423 : 0 : &( type ), &( time_buff[0] ),
424 : 0 : &( pack->status ), &( pack->lat ), &( pack->ns ), &( pack->lon ), &( pack->ew ),
425 : 0 : &( pack->speed ), &( pack->direction ),
426 : 0 : &( pack->utc.day ), &( pack->utc.mon ), &( pack->utc.year ),
427 : 0 : &( pack->declination ), &( pack->declin_ew ), &( pack->mode ) );
428 : :
429 : 0 : if ( nsen != 14 && nsen != 15 )
430 : : {
431 : 0 : nmea_error( "G?RMC parse error!" );
432 : 0 : return 0;
433 : : }
434 : :
435 : 0 : if ( type != 'P' && type != 'N' )
436 : : {
437 : 0 : nmea_error( "G?RMC invalid type " );
438 : 0 : return 0;
439 : : }
440 : :
441 : 0 : if ( 0 != _nmea_parse_time( &time_buff[0], ( int )strlen( &time_buff[0] ), &( pack->utc ) ) )
442 : : {
443 : 0 : nmea_error( "GPRMC time parse error!" );
444 : 0 : return 0;
445 : : }
446 : :
447 : 0 : if ( pack->utc.year < 90 )
448 : 0 : pack->utc.year += 100;
449 : 0 : pack->utc.mon -= 1;
450 : :
451 : 0 : return 1;
452 : 0 : }
453 : :
454 : : /**
455 : : * \brief Parse HDT packet from buffer.
456 : : * @param buff a constant character pointer of packet buffer.
457 : : * @param buff_sz buffer size.
458 : : * @param pack a pointer of packet which will filled by function.
459 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
460 : : */
461 : 0 : int nmea_parse_GPHDT( const char *buff, int buff_sz, nmeaGPHDT *pack )
462 : : {
463 : 0 : NMEA_ASSERT( buff && pack );
464 : :
465 : 0 : memset( pack, 0, sizeof( nmeaGPHDT ) );
466 : :
467 : 0 : nmea_trace_buff( buff, buff_sz );
468 : :
469 : : char type;
470 : : char talker_id;
471 : :
472 : 0 : if ( 3 != nmea_scanf( buff, buff_sz,
473 : : "$G%CHDT,%f,%C*",
474 : : &( talker_id ),
475 : 0 : &( pack->heading ), &( type ) ) )
476 : : {
477 : 0 : nmea_error( "G?HDT parse error!" );
478 : 0 : return 0;
479 : : }
480 : :
481 : 0 : if ( talker_id != 'P' && talker_id != 'N' )
482 : : {
483 : 0 : nmea_error( "G?HDT invalid type " );
484 : 0 : return 0;
485 : : }
486 : :
487 : 0 : if ( type != 'T' )
488 : : {
489 : 0 : nmea_error( "G?HDT invalid type " );
490 : 0 : return 0;
491 : : }
492 : :
493 : 0 : return 1;
494 : 0 : }
495 : :
496 : : /**
497 : : * \brief Parse VTG packet from buffer.
498 : : * @param buff a constant character pointer of packet buffer.
499 : : * @param buff_sz buffer size.
500 : : * @param pack a pointer of packet which will filled by function.
501 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
502 : : */
503 : 0 : int nmea_parse_GPVTG( const char *buff, int buff_sz, nmeaGPVTG *pack )
504 : : {
505 : 0 : NMEA_ASSERT( buff && pack );
506 : :
507 : 0 : memset( pack, 0, sizeof( nmeaGPVTG ) );
508 : :
509 : 0 : nmea_trace_buff( buff, buff_sz );
510 : :
511 : : char type;
512 : :
513 : 0 : if ( 9 != nmea_scanf( buff, buff_sz,
514 : : "$G%CVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
515 : : &type,
516 : 0 : &( pack->dir ), &( pack->dir_t ),
517 : 0 : &( pack->dec ), &( pack->dec_m ),
518 : 0 : &( pack->spn ), &( pack->spn_n ),
519 : 0 : &( pack->spk ), &( pack->spk_k ) ) )
520 : : {
521 : 0 : nmea_error( "G?VTG parse error!" );
522 : 0 : return 0;
523 : : }
524 : :
525 : 0 : if ( type != 'P' && type != 'N' )
526 : : {
527 : 0 : nmea_error( "G?VTG invalid type " );
528 : 0 : return 0;
529 : : }
530 : :
531 : 0 : if ( pack->dir_t != 'T' ||
532 : 0 : pack->dec_m != 'M' ||
533 : 0 : pack->spn_n != 'N' ||
534 : 0 : pack->spk_k != 'K' )
535 : : {
536 : 0 : nmea_error( "G?VTG parse error (format error)!" );
537 : 0 : return 0;
538 : : }
539 : :
540 : 0 : return 1;
541 : 0 : }
542 : :
543 : : /**
544 : : * \brief Parse HCHDG packet from buffer.
545 : : * @param buff a constant character pointer of packet buffer.
546 : : * @param buff_sz buffer size.
547 : : * @param pack a pointer of packet which will filled by function.
548 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
549 : : */
550 : 0 : int nmea_parse_HCHDG( const char *buff, int buff_sz, nmeaHCHDG *pack )
551 : : {
552 : 0 : NMEA_ASSERT( buff && pack );
553 : :
554 : 0 : memset( pack, 0, sizeof( nmeaHCHDG ) );
555 : :
556 : 0 : nmea_trace_buff( buff, buff_sz );
557 : :
558 : 0 : if ( 5 != nmea_scanf( buff, buff_sz,
559 : : "$HCHDG,%f,%f,%C,%f,%C*",
560 : 0 : &( pack->mag_heading ), &( pack->mag_deviation ),
561 : 0 : &( pack->ew_deviation ), &( pack->mag_variation ),
562 : 0 : &( pack->ew_variation ) ) )
563 : : {
564 : 0 : nmea_error( "HCHDG parse error!" );
565 : 0 : return 0;
566 : : }
567 : :
568 : 0 : if ( pack->ew_deviation != 'E' && pack->ew_deviation != 'W' )
569 : : {
570 : 0 : nmea_error( "HCHDG invalid deviation direction" );
571 : 0 : return 0;
572 : : }
573 : :
574 : 0 : if ( pack->ew_variation != 'E' && pack->ew_variation != 'W' )
575 : : {
576 : 0 : nmea_error( "HCHDG invalid variation direction" );
577 : 0 : return 0;
578 : : }
579 : :
580 : 0 : return 1;
581 : 0 : }
582 : :
583 : : /**
584 : : * \brief Parse HDT packet from buffer.
585 : : * @param buff a constant character pointer of packet buffer.
586 : : * @param buff_sz buffer size.
587 : : * @param pack a pointer of packet which will filled by function.
588 : : * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
589 : : */
590 : 0 : int nmea_parse_HCHDT( const char *buff, int buff_sz, nmeaHCHDT *pack )
591 : : {
592 : 0 : NMEA_ASSERT( buff && pack );
593 : :
594 : 0 : memset( pack, 0, sizeof( nmeaHCHDT ) );
595 : :
596 : 0 : nmea_trace_buff( buff, buff_sz );
597 : :
598 : 0 : if ( 2 != nmea_scanf( buff, buff_sz,
599 : : "$HCHDT,%f,%C*",
600 : 0 : &( pack->direction ), &( pack->t_flag ) ) )
601 : : {
602 : 0 : nmea_error( "HCHDT parse error!" );
603 : 0 : return 0;
604 : : }
605 : :
606 : 0 : return 1;
607 : 0 : }
608 : :
609 : : /**
610 : : * \brief Fill nmeaINFO structure by GGA packet data.
611 : : * @param pack a pointer of packet structure.
612 : : * @param info a pointer of summary information structure.
613 : : */
614 : 0 : void nmea_GPGGA2info( nmeaGPGGA *pack, nmeaINFO *info )
615 : : {
616 : 0 : NMEA_ASSERT( pack && info );
617 : :
618 : 0 : info->utc.hour = pack->utc.hour;
619 : 0 : info->utc.min = pack->utc.min;
620 : 0 : info->utc.sec = pack->utc.sec;
621 : 0 : info->utc.msec = pack->utc.msec;
622 : 0 : info->sig = pack->sig;
623 : 0 : info->HDOP = pack->HDOP;
624 : 0 : info->elv = pack->elv;
625 : 0 : info->lat = ( ( pack->ns == 'N' ) ? pack->lat : -( pack->lat ) );
626 : 0 : info->lon = ( ( pack->ew == 'E' ) ? pack->lon : -( pack->lon ) );
627 : 0 : info->smask |= GPGGA;
628 : 0 : }
629 : :
630 : : /**
631 : : * \brief Fill nmeaINFO structure by GST packet data.
632 : : * @param pack a pointer of packet structure.
633 : : * @param info a pointer of summary information structure.
634 : : */
635 : 0 : void nmea_GPGST2info( nmeaGPGST *pack, nmeaINFO *info )
636 : : {
637 : 0 : NMEA_ASSERT( pack && info );
638 : :
639 : 0 : info->utc.hour = pack->utc.hour;
640 : 0 : info->utc.min = pack->utc.min;
641 : 0 : info->utc.sec = pack->utc.sec;
642 : 0 : info->utc.msec = pack->utc.msec;
643 : 0 : info->rms_pr = pack->rms_pr;
644 : 0 : info->err_major = pack->err_major;
645 : 0 : info->err_minor = pack->err_minor;
646 : 0 : info->err_ori = pack->err_ori;
647 : 0 : info->sig_lat = pack->sig_lat;
648 : 0 : info->sig_lon = pack->sig_lon;
649 : 0 : info->sig_alt = pack->sig_alt;
650 : 0 : info->smask |= GPGST;
651 : 0 : }
652 : :
653 : : /**
654 : : * \brief Fill nmeaINFO structure by GSA packet data.
655 : : * @param pack a pointer of packet structure.
656 : : * @param info a pointer of summary information structure.
657 : : */
658 : 0 : void nmea_GPGSA2info( nmeaGPGSA *pack, nmeaINFO *info )
659 : : {
660 : 0 : int i, j, nuse = 0;
661 : :
662 : 0 : NMEA_ASSERT( pack && info );
663 : :
664 : 0 : info->fix = pack->fix_type;
665 : 0 : info->PDOP = pack->PDOP;
666 : 0 : info->HDOP = pack->HDOP;
667 : 0 : info->VDOP = pack->VDOP;
668 : :
669 : 0 : for ( i = 0; i < NMEA_MAXSAT; ++i )
670 : : {
671 : 0 : for ( j = 0; j < info->satinfo.inview; ++j )
672 : : {
673 : 0 : if ( pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id )
674 : : {
675 : 0 : info->satinfo.sat[j].in_use = 1;
676 : 0 : nuse++;
677 : 0 : }
678 : 0 : }
679 : 0 : }
680 : :
681 : 0 : info->satinfo.inuse = nuse;
682 : 0 : info->smask |= GPGSA;
683 : 0 : }
684 : :
685 : : /**
686 : : * \brief Fill nmeaINFO structure by GSV packet data.
687 : : * @param pack a pointer of packet structure.
688 : : * @param info a pointer of summary information structure.
689 : : */
690 : 0 : void nmea_GPGSV2info( nmeaGPGSV *pack, nmeaINFO *info )
691 : : {
692 : : int isat, isi, nsat;
693 : :
694 : 0 : NMEA_ASSERT( pack && info );
695 : :
696 : 0 : if ( pack->pack_index > pack->pack_count ||
697 : 0 : pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT )
698 : 0 : return;
699 : :
700 : 0 : if ( pack->pack_index < 1 )
701 : 0 : pack->pack_index = 1;
702 : :
703 : 0 : info->satinfo.inview = pack->sat_count;
704 : :
705 : 0 : nsat = ( pack->pack_index - 1 ) * NMEA_SATINPACK;
706 : 0 : nsat = ( nsat + NMEA_SATINPACK > pack->sat_count ) ? pack->sat_count - nsat : NMEA_SATINPACK;
707 : :
708 : 0 : for ( isat = 0; isat < nsat; ++isat )
709 : : {
710 : 0 : isi = ( pack->pack_index - 1 ) * NMEA_SATINPACK + isat;
711 : 0 : info->satinfo.sat[isi].id = pack->sat_data[isat].id;
712 : 0 : info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
713 : 0 : info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
714 : 0 : info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
715 : 0 : }
716 : :
717 : 0 : info->smask |= GPGSV;
718 : 0 : }
719 : :
720 : : /**
721 : : * \brief Fill nmeaINFO structure by RMC packet data.
722 : : * @param pack a pointer of packet structure.
723 : : * @param info a pointer of summary information structure.
724 : : */
725 : 0 : void nmea_GPRMC2info( nmeaGPRMC *pack, nmeaINFO *info )
726 : : {
727 : 0 : NMEA_ASSERT( pack && info );
728 : :
729 : 0 : if ( 'A' == pack->status )
730 : : {
731 : 0 : if ( NMEA_SIG_BAD == info->sig )
732 : 0 : info->sig = NMEA_SIG_MID;
733 : 0 : if ( NMEA_FIX_BAD == info->fix )
734 : 0 : info->fix = NMEA_FIX_2D;
735 : 0 : }
736 : 0 : else if ( 'V' == pack->status )
737 : : {
738 : 0 : info->sig = NMEA_SIG_BAD;
739 : 0 : info->fix = NMEA_FIX_BAD;
740 : 0 : }
741 : :
742 : 0 : info->utc = pack->utc;
743 : 0 : info->lat = ( ( pack->ns == 'N' ) ? pack->lat : -( pack->lat ) );
744 : 0 : info->lon = ( ( pack->ew == 'E' ) ? pack->lon : -( pack->lon ) );
745 : 0 : info->speed = pack->speed * NMEA_TUD_KNOTS;
746 : 0 : info->direction = pack->direction;
747 : 0 : info->smask |= GPRMC;
748 : 0 : }
749 : :
750 : : /**
751 : : * \brief Fill nmeaINFO structure by VTG packet data.
752 : : * @param pack a pointer of packet structure.
753 : : * @param info a pointer of summary information structure.
754 : : */
755 : 0 : void nmea_GPVTG2info( nmeaGPVTG *pack, nmeaINFO *info )
756 : : {
757 : 0 : NMEA_ASSERT( pack && info );
758 : :
759 : 0 : info->direction = pack->dir;
760 : 0 : info->declination = pack->dec;
761 : 0 : info->speed = pack->spk;
762 : 0 : info->smask |= GPVTG;
763 : 0 : }
|
