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
31 #include <cfg/debug.h>
34 #include "../inc/nmeap.h"
36 #define assert(x) ASSERT(x)
39 * Removed to compile in BeRTOS environment.
44 * #include <cfg/debug.h>
53 /* this only works if you are sure you have an upper case hex digit */
54 #define HEXTOBIN(ch) ((ch <= '9') ? ch - '0' : ch - ('A' - 10))
56 /* forward references */
57 int nmeap_init(nmeap_context_t *context,void *user_data);
58 int nmeap_addParser(nmeap_context_t *context,
59 const char *sentence_name,
60 nmeap_sentence_parser_t sentence_parser,
61 nmeap_callout_t sentence_callout,
64 int nmeap_tokenize(nmeap_context_t *context);
65 int nmeap_process(nmeap_context_t *context);
66 int nmeap_parse(nmeap_context_t *context,char ch);
67 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length);
70 * get a latitude out of a pair of nmea tokens
72 double nmeap_latitude(const char *plat,const char *phem)
89 /* north lat is +, south lat is - */
97 /* latitude is degrees, minutes, fractional minutes */
98 /* no validation is performed on the token. it better be good.*/
99 /* if it comes back 0.0 then probably the token was bad */
102 /* extract the degree part */
103 deg = (int)(lat / 100.0);
105 /* mask out the degrees */
106 min = lat - (deg * 100.0);
108 /* compute the actual latitude in degrees.decimal-degrees */
109 lat = (deg + (min / 60.0)) * ns;
115 * get a longitude out of a pair of nmea tokens
117 double nmeap_longitude(const char *plon,const char *phem)
134 /* west long is negative, east long is positive */
142 /* longitude is degrees, minutes, fractional minutes */
143 /* no validation is performed on the token. it better be good.*/
144 /* if it comes back 0.0 then probably the token was bad */
147 /* extract the degree part */
148 deg = (int)(lon / 100.0);
150 /* mask out the degrees */
151 min = lon - (deg * 100.0);
153 /* compute the actual lonitude in degrees.decimal-degrees */
154 lon = (deg + (min / 60.0)) * ew;
161 * get an altitude longitude out of a pair of nmea tokens
162 * ALTITUDE is returned in METERS
164 double nmeap_altitude(const char *palt,const char *punits)
172 /* convert with no error checking */
175 if (*punits == 'M') {
176 /* already in meters */
178 else if (*punits == 'F') {
179 /* convert to feet */
180 alt = alt * 3.2808399;
187 * initialize an NMEA parser
189 int nmeap_init(nmeap_context_t *context,void *user_data)
191 assert(context != 0);
193 memset(context,0,sizeof(*context));
195 context->user_data = user_data;
201 * register an NMEA sentence parser
203 int nmeap_addParser(nmeap_context_t *context,
204 const char *sentence_name,
205 nmeap_sentence_parser_t sentence_parser,
206 nmeap_callout_t sentence_callout,
210 nmeap_sentence_t *s = 0;
213 assert(context != 0);
215 /* sentence capacity overflow */
216 if (context->sentence_count >= NMEAP_MAX_SENTENCES) {
220 /* point at next empty sentence buffer */
221 s = &context->sentence[context->sentence_count];
223 /* advance sentence data count */
224 context->sentence_count++;
226 /* clear the sentence data */
227 memset(s,0,sizeof(*s));
230 strncpy(s->name,sentence_name,NMEAP_MAX_SENTENCE_NAME_LENGTH);
233 s->parser = sentence_parser;
236 s->callout = sentence_callout;
239 s->data = sentence_data;
247 int nmeap_tokenize(nmeap_context_t *context)
253 /* first token is header. assume it is there */
256 context->token[tokens] = s;
258 /* get rest of tokens */
261 while((*s != 0)&&(tokens < NMEAP_MAX_TOKENS)) {
264 /* looking for end of a token */
266 /* delimit at the comma */
273 /* start of next token, might be another comma */
274 context->token[tokens++] = s;
276 /* delimit at the comma */
298 int nmeap_process(nmeap_context_t *context)
304 /* copy the input to a debug buffer */
305 /* remove debug_input when everything is working. */
306 strncpy(context->debug_input,context->input,sizeof(context->debug_input));
308 /* tokenize the input */
309 context->tokens = nmeap_tokenize(context);
311 /* try to find a matching sentence parser */
312 /* 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 */
313 /* binary search instead of linear (have to keep sentences in sorted order) O(NlogN) */
314 /* OR, when sentences are added, create a TRIE structure to find the names with a constant time search O(5) */
315 for(i=0;i<context->sentence_count;i++) {
316 s = &context->sentence[i];
318 if (strncmp(context->input_name,s->name,5) == 0) {
319 /* found a match, call its parser */
320 id = (*context->sentence[i].parser)(context,s);
333 +------+ +------+ +------+ +------+ +------+
334 | 0 |--$--> |1-hdr |--alnum--> |2-data|----\r-->| 6-LF |---\n--->| done |--> 0
335 +------+ +------+ +------+ +------+ +------+
337 * +--------\r-------+
339 +------+ +------+ +------+
340 |3-cks |--xdigit-->|4-cks |-xdigit->| 5-CR |
341 +------+ +------+ +------+
343 return to start conditions:
345 2. invalid character for state
348 two hex digits represent the XOR of all characters between, but not
349 including, the "$" and "*". A checksum is required on some
353 int nmeap_parse(nmeap_context_t *context,char ch)
357 /* check for input buffer overrun first to avoid duplicating code in the
360 if ((size_t)context->input_count >= (sizeof(context->input)-1)) {
361 /* input buffer overrun, restart state machine */
362 context->input_state = 0;
363 /* reset input count */
364 context->input_count = 0;
368 context->input[context->input_count] = ch;
370 /* next buffer position */
371 context->input_count++;
373 /* run it through the lexical scanner */
374 switch(context->input_state) {
379 context->input_state = 1;
384 /* header error, start over */
386 context->input_state = 0;
387 context->input_count = 0;
390 /* LOOKING FOR 5 CHARACTER SENTENCE ID */
392 /* allow numbers even though it isn't usually done */
393 /* a proprietary id might have a numeral */
395 /* store name separately */
396 context->input_name[context->input_count - 2] = ch;
400 if (context->input_count >= 6) {
402 context->input_state = 2;
406 /* bad character, start over */
408 context->input_state = 0;
409 context->input_count = 0;
412 /* LOOKING FOR CR OR CHECKSUM INDICATOR */
415 /* this sentence has a checksum */
416 context->input_state = 3;
418 else if (ch == '\r') {
419 /* carriage return, no checksum, force a match */
422 context->input_state = 6;
425 /* continue accumulating data */
430 /* LOOKING FOR FIRST CHECKSUM CHARACTER */
432 /* must be upper case hex digit */
433 if (isxdigit(ch) && (ch <= 'F')) {
434 /* got first checksum byte */
435 context->input_state = 4;
436 context->icks = HEXTOBIN(ch) << 4;
439 /* input error, restart */
441 context->input_state = 0;
442 context->input_count = 0;
445 /* LOOKING FOR SECOND CHECKSUM CHARACTER */
447 /* must be upper case hex digit */
448 if (isxdigit(ch) && (ch <= 'F')) {
449 /* got second checksum byte */
450 context->input_state = 5;
451 context->icks += HEXTOBIN(ch);
454 /* input error, restart */
456 context->input_state = 0;
457 context->input_count = 0;
463 /* carriage return */
464 context->input_state = 6;
467 /* input error, restart */
469 context->input_state = 0;
470 context->input_count = 0;
473 /* LOOKING FOR LINE FEED */
476 /* linefeed, line complete */
479 context->input[context->input_count] = 0;
481 /* if the checksums match, process the sentence */
482 if (context->ccks == context->icks) {
484 status = nmeap_process(context);
486 /* count good messages */
490 /* count checksum errors */
494 /* restart next time */
495 context->input_state = 0;
496 context->input_count = 0;
499 /* input error, restart */
501 context->input_state = 0;
502 context->input_count = 0;
507 context->input_state = 0;
515 * parse a buffer of nmea data
517 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length)
527 /* for each byte in the buffer */
528 for(i=0;i<tlen;i++) {
529 /* decrement remaining byte count */
532 status = nmeap_parse(context,buffer[i]);
534 /* message found or error */
539 /* return remaining byte count */
546 * standard GPGGA sentence parser
548 int nmeap_gpgga(nmeap_context_t *context,nmeap_sentence_t *sentence)
554 /* get pointer to sentence data */
555 nmeap_gga_t *gga = (nmeap_gga_t *)sentence->data;
557 /* if there is a data element, extract data from the tokens */
559 gga->latitude = nmeap_latitude(context->token[2],context->token[3]);
560 gga->longitude = nmeap_longitude(context->token[4],context->token[5]);
561 gga->altitude = nmeap_altitude(context->token[9],context->token[10]);
562 gga->time = atoi(context->token[1]);
563 gga->satellites = atoi(context->token[7]);
564 gga->quality = atoi(context->token[6]);
565 gga->hdop = atof(context->token[8]);
566 gga->geoid = nmeap_altitude(context->token[11],context->token[12]);
570 /* print raw input string */
571 printf("%s",context->debug_input);
573 /* print some validation data */
574 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
576 /* print the tokens */
577 for(i=0;i<context->tokens;i++) {
578 printf("%d:%s\n",i,context->token[i]);
582 /* if the sentence has a callout, call it */
583 if (sentence->callout != 0) {
584 (*sentence->callout)(context,gga,context->user_data);
591 * standard GPRMCntence parser
593 int nmeap_gprmc(nmeap_context_t *context,nmeap_sentence_t *sentence)
599 /* get pointer to sentence data */
600 nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->data;
602 /* if there is a data element, use it */
604 /* extract data from the tokens */
605 rmc->time = atoi(context->token[1]);
606 rmc->warn = *context->token[2];
607 rmc->latitude = nmeap_latitude(context->token[3],context->token[4]);
608 rmc->longitude = nmeap_longitude(context->token[5],context->token[6]);
609 rmc->speed = atof(context->token[7]);
610 rmc->course = atof(context->token[8]);
611 rmc->date = atoi(context->token[9]);
612 rmc->magvar = atof(context->token[10]);
616 /* print raw input string */
617 printf("%s",context->debug_input);
619 /* print some validation data */
620 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
622 /* print the tokens */
623 for(i=0;i<context->tokens;i++) {
624 printf("%d:%s\n",i,context->token[i]);
628 /* if the sentence has a callout, call it */
629 if (sentence->callout != 0) {
630 (*sentence->callout)(context,rmc,context->user_data);