2 Copyright (c) 2005, David M Howard (daveh at dmh2000.com)
5 This product is licensed for use and distribution under the BSD Open Source License.
6 see the file COPYING for more details.
8 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
9 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
10 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
11 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
12 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
13 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
14 OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
15 OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
16 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
17 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
18 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 * nmeap gps data parser
26 * see the file COPYING for terms of the licnese
34 #include "../inc/nmeap.h"
36 #include <cfg/debug.h>
38 #define assert(x) ASSERT(x)
40 #include "cfg/cfg_nmea.h"
42 #define LOG_LEVEL NMEA_LOG_LEVEL
43 #define LOG_FORMAT NMEA_LOG_FORMAT
48 #define printf(str,...) LOG_INFO(str, ## __VA_ARGS__)
51 /* this only works if you are sure you have an upper case hex digit */
52 #define HEXTOBIN(ch) ((ch <= '9') ? ch - '0' : ch - ('A' - 10))
54 /* forward references */
55 int nmeap_init(nmeap_context_t *context,void *user_data);
56 int nmeap_addParser(nmeap_context_t *context,
57 const char *sentence_name,
58 nmeap_sentence_parser_t sentence_parser,
59 nmeap_callout_t sentence_callout,
62 int nmeap_tokenize(nmeap_context_t *context);
63 int nmeap_process(nmeap_context_t *context);
64 int nmeap_parse(nmeap_context_t *context,char ch);
65 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length);
68 * get a latitude out of a pair of nmea tokens
70 double nmeap_latitude(const char *plat,const char *phem)
87 /* north lat is +, south lat is - */
95 /* latitude is degrees, minutes, fractional minutes */
96 /* no validation is performed on the token. it better be good.*/
97 /* if it comes back 0.0 then probably the token was bad */
100 /* extract the degree part */
101 deg = (int)(lat / 100.0);
103 /* mask out the degrees */
104 min = lat - (deg * 100.0);
106 /* compute the actual latitude in degrees.decimal-degrees */
107 lat = (deg + (min / 60.0)) * ns;
113 * get a longitude out of a pair of nmea tokens
115 double nmeap_longitude(const char *plon,const char *phem)
132 /* west long is negative, east long is positive */
140 /* longitude is degrees, minutes, fractional minutes */
141 /* no validation is performed on the token. it better be good.*/
142 /* if it comes back 0.0 then probably the token was bad */
145 /* extract the degree part */
146 deg = (int)(lon / 100.0);
148 /* mask out the degrees */
149 min = lon - (deg * 100.0);
151 /* compute the actual lonitude in degrees.decimal-degrees */
152 lon = (deg + (min / 60.0)) * ew;
159 * get an altitude longitude out of a pair of nmea tokens
160 * ALTITUDE is returned in METERS
162 double nmeap_altitude(const char *palt,const char *punits)
170 /* convert with no error checking */
173 if (*punits == 'M') {
174 /* already in meters */
176 else if (*punits == 'F') {
177 /* convert to feet */
178 alt = alt * 3.2808399;
185 * initialize an NMEA parser
187 int nmeap_init(nmeap_context_t *context,void *user_data)
189 assert(context != 0);
191 memset(context,0,sizeof(*context));
193 context->user_data = user_data;
199 * register an NMEA sentence parser
201 int nmeap_addParser(nmeap_context_t *context,
202 const char *sentence_name,
203 nmeap_sentence_parser_t sentence_parser,
204 nmeap_callout_t sentence_callout,
208 nmeap_sentence_t *s = 0;
211 assert(context != 0);
213 /* sentence capacity overflow */
214 if (context->sentence_count >= NMEAP_MAX_SENTENCES) {
218 /* point at next empty sentence buffer */
219 s = &context->sentence[context->sentence_count];
221 /* advance sentence data count */
222 context->sentence_count++;
224 /* clear the sentence data */
225 memset(s,0,sizeof(*s));
228 strncpy(s->name,sentence_name,NMEAP_MAX_SENTENCE_NAME_LENGTH);
231 s->parser = sentence_parser;
234 s->callout = sentence_callout;
237 s->data = sentence_data;
245 int nmeap_tokenize(nmeap_context_t *context)
251 /* first token is header. assume it is there */
254 context->token[tokens] = s;
256 /* get rest of tokens */
259 while((*s != 0)&&(tokens < NMEAP_MAX_TOKENS)) {
262 /* looking for end of a token */
264 /* delimit at the comma */
271 /* start of next token, might be another comma */
272 context->token[tokens++] = s;
274 /* delimit at the comma */
296 int nmeap_process(nmeap_context_t *context)
302 /* copy the input to a debug buffer */
303 /* remove debug_input when everything is working. */
304 strncpy(context->debug_input,context->input,sizeof(context->debug_input));
306 /* tokenize the input */
307 context->tokens = nmeap_tokenize(context);
309 /* try to find a matching sentence parser */
310 /* this search is O(n). it has a lot of potential for optimization, at the expense of complexity, if you have a lot of sentences */
311 /* binary search instead of linear (have to keep sentences in sorted order) O(NlogN) */
312 /* OR, when sentences are added, create a TRIE structure to find the names with a constant time search O(5) */
313 for(i=0;i<context->sentence_count;i++) {
314 s = &context->sentence[i];
316 if (strncmp(context->input_name,s->name,5) == 0) {
317 /* found a match, call its parser */
318 id = (*context->sentence[i].parser)(context,s);
331 +------+ +------+ +------+ +------+ +------+
332 | 0 |--$--> |1-hdr |--alnum--> |2-data|----\r-->| 6-LF |---\n--->| done |--> 0
333 +------+ +------+ +------+ +------+ +------+
335 * +--------\r-------+
337 +------+ +------+ +------+
338 |3-cks |--xdigit-->|4-cks |-xdigit->| 5-CR |
339 +------+ +------+ +------+
341 return to start conditions:
343 2. invalid character for state
346 two hex digits represent the XOR of all characters between, but not
347 including, the "$" and "*". A checksum is required on some
351 int nmeap_parse(nmeap_context_t *context,char ch)
355 /* check for input buffer overrun first to avoid duplicating code in the
358 if ((size_t)context->input_count >= (sizeof(context->input)-1)) {
359 /* input buffer overrun, restart state machine */
360 context->input_state = 0;
361 /* reset input count */
362 context->input_count = 0;
366 context->input[context->input_count] = ch;
368 /* next buffer position */
369 context->input_count++;
371 /* run it through the lexical scanner */
372 switch(context->input_state) {
377 context->input_state = 1;
382 /* header error, start over */
384 context->input_state = 0;
385 context->input_count = 0;
388 /* LOOKING FOR 5 CHARACTER SENTENCE ID */
390 /* allow numbers even though it isn't usually done */
391 /* a proprietary id might have a numeral */
393 /* store name separately */
394 context->input_name[context->input_count - 2] = ch;
398 if (context->input_count >= 6) {
400 context->input_state = 2;
404 /* bad character, start over */
406 context->input_state = 0;
407 context->input_count = 0;
410 /* LOOKING FOR CR OR CHECKSUM INDICATOR */
413 /* this sentence has a checksum */
414 context->input_state = 3;
416 else if (ch == '\r') {
417 /* carriage return, no checksum, force a match */
420 context->input_state = 6;
423 /* continue accumulating data */
428 /* LOOKING FOR FIRST CHECKSUM CHARACTER */
430 /* must be upper case hex digit */
431 if (isxdigit(ch) && (ch <= 'F')) {
432 /* got first checksum byte */
433 context->input_state = 4;
434 context->icks = HEXTOBIN(ch) << 4;
437 /* input error, restart */
439 context->input_state = 0;
440 context->input_count = 0;
443 /* LOOKING FOR SECOND CHECKSUM CHARACTER */
445 /* must be upper case hex digit */
446 if (isxdigit(ch) && (ch <= 'F')) {
447 /* got second checksum byte */
448 context->input_state = 5;
449 context->icks += HEXTOBIN(ch);
452 /* input error, restart */
454 context->input_state = 0;
455 context->input_count = 0;
461 /* carriage return */
462 context->input_state = 6;
465 /* input error, restart */
467 context->input_state = 0;
468 context->input_count = 0;
471 /* LOOKING FOR LINE FEED */
474 /* linefeed, line complete */
477 context->input[context->input_count] = 0;
479 /* if the checksums match, process the sentence */
480 if (context->ccks == context->icks) {
482 status = nmeap_process(context);
484 /* count good messages */
488 /* count checksum errors */
492 /* restart next time */
493 context->input_state = 0;
494 context->input_count = 0;
497 /* input error, restart */
499 context->input_state = 0;
500 context->input_count = 0;
505 context->input_state = 0;
513 * parse a buffer of nmea data
515 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length)
525 /* for each byte in the buffer */
526 for(i=0;i<tlen;i++) {
527 /* decrement remaining byte count */
530 status = nmeap_parse(context,buffer[i]);
532 /* message found or error */
537 /* return remaining byte count */
544 * standard GPGGA sentence parser
546 int nmeap_gpgga(nmeap_context_t *context,nmeap_sentence_t *sentence)
552 /* get pointer to sentence data */
553 nmeap_gga_t *gga = (nmeap_gga_t *)sentence->data;
555 /* if there is a data element, extract data from the tokens */
557 gga->latitude = nmeap_latitude(context->token[2],context->token[3]);
558 gga->longitude = nmeap_longitude(context->token[4],context->token[5]);
559 gga->altitude = nmeap_altitude(context->token[9],context->token[10]);
560 gga->time = atoi(context->token[1]);
561 gga->satellites = atoi(context->token[7]);
562 gga->quality = atoi(context->token[6]);
563 gga->hdop = atof(context->token[8]);
564 gga->geoid = nmeap_altitude(context->token[11],context->token[12]);
568 /* print raw input string */
569 printf("%s",context->debug_input);
571 /* print some validation data */
572 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
574 /* print the tokens */
575 for(i=0;i<context->tokens;i++) {
576 printf("%d:%s\n",i,context->token[i]);
580 /* if the sentence has a callout, call it */
581 if (sentence->callout != 0) {
582 (*sentence->callout)(context,gga,context->user_data);
589 * standard GPRMCntence parser
591 int nmeap_gprmc(nmeap_context_t *context,nmeap_sentence_t *sentence)
597 /* get pointer to sentence data */
598 nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->data;
600 /* if there is a data element, use it */
602 /* extract data from the tokens */
603 rmc->time = atoi(context->token[1]);
604 rmc->warn = *context->token[2];
605 rmc->latitude = nmeap_latitude(context->token[3],context->token[4]);
606 rmc->longitude = nmeap_longitude(context->token[5],context->token[6]);
607 rmc->speed = atof(context->token[7]);
608 rmc->course = atof(context->token[8]);
609 rmc->date = atoi(context->token[9]);
610 rmc->magvar = atof(context->token[10]);
614 /* print raw input string */
615 printf("%s",context->debug_input);
617 /* print some validation data */
618 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
620 /* print the tokens */
621 for(i=0;i<context->tokens;i++) {
622 printf("%d:%s\n",i,context->token[i]);
626 /* if the sentence has a callout, call it */
627 if (sentence->callout != 0) {
628 (*sentence->callout)(context,rmc,context->user_data);