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
37 /* this only works if you are sure you have an upper case hex digit */
38 #define HEXTOBIN(ch) ((ch <= '9') ? ch - '0' : ch - ('A' - 10))
40 /* forward references */
41 int nmeap_init(nmeap_context_t *context,void *user_data);
42 int nmeap_addParser(nmeap_context_t *context,
43 const char *sentence_name,
44 nmeap_sentence_parser_t sentence_parser,
45 nmeap_callout_t sentence_callout,
48 int nmeap_tokenize(nmeap_context_t *context);
49 int nmeap_process(nmeap_context_t *context);
50 int nmeap_parse(nmeap_context_t *context,char ch);
51 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length);
54 * get a latitude out of a pair of nmea tokens
56 double nmeap_latitude(const char *plat,const char *phem)
73 /* north lat is +, south lat is - */
81 /* latitude is degrees, minutes, fractional minutes */
82 /* no validation is performed on the token. it better be good.*/
83 /* if it comes back 0.0 then probably the token was bad */
86 /* extract the degree part */
87 deg = (int)(lat / 100.0);
89 /* mask out the degrees */
90 min = lat - (deg * 100.0);
92 /* compute the actual latitude in degrees.decimal-degrees */
93 lat = (deg + (min / 60.0)) * ns;
99 * get a longitude out of a pair of nmea tokens
101 double nmeap_longitude(const char *plon,const char *phem)
118 /* west long is negative, east long is positive */
126 /* longitude is degrees, minutes, fractional minutes */
127 /* no validation is performed on the token. it better be good.*/
128 /* if it comes back 0.0 then probably the token was bad */
131 /* extract the degree part */
132 deg = (int)(lon / 100.0);
134 /* mask out the degrees */
135 min = lon - (deg * 100.0);
137 /* compute the actual lonitude in degrees.decimal-degrees */
138 lon = (deg + (min / 60.0)) * ew;
145 * get an altitude longitude out of a pair of nmea tokens
146 * ALTITUDE is returned in METERS
148 double nmeap_altitude(const char *palt,const char *punits)
156 /* convert with no error checking */
159 if (*punits == 'M') {
160 /* already in meters */
162 else if (*punits == 'F') {
163 /* convert to feet */
164 alt = alt * 3.2808399;
171 * initialize an NMEA parser
173 int nmeap_init(nmeap_context_t *context,void *user_data)
175 assert(context != 0);
177 memset(context,0,sizeof(*context));
179 context->user_data = user_data;
186 * register an NMEA sentence parser
188 int nmeap_addParser(nmeap_context_t *context,
189 const char *sentence_name,
190 nmeap_sentence_parser_t sentence_parser,
191 nmeap_callout_t sentence_callout,
195 nmeap_sentence_t *s = 0;
198 assert(context != 0);
200 /* sentence capacity overflow */
201 if (context->sentence_count >= NMEAP_MAX_SENTENCES) {
205 /* point at next empty sentence buffer */
206 s = &context->sentence[context->sentence_count];
208 /* advance sentence data count */
209 context->sentence_count++;
211 /* clear the sentence data */
212 memset(s,0,sizeof(*s));
215 strncpy(s->name,sentence_name,NMEAP_MAX_SENTENCE_NAME_LENGTH);
218 s->parser = sentence_parser;
221 s->callout = sentence_callout;
224 s->data = sentence_data;
232 int nmeap_tokenize(nmeap_context_t *context)
238 /* first token is header. assume it is there */
241 context->token[tokens] = s;
243 /* get rest of tokens */
246 while((*s != 0)&&(tokens < NMEAP_MAX_TOKENS)) {
249 /* looking for end of a token */
251 /* delimit at the comma */
258 /* start of next token, might be another comma */
259 context->token[tokens++] = s;
261 /* delimit at the comma */
283 int nmeap_process(nmeap_context_t *context)
289 /* copy the input to a debug buffer */
290 /* remove debug_input when everything is working. */
291 strncpy(context->debug_input,context->input,sizeof(context->debug_input));
293 /* tokenize the input */
294 context->tokens = nmeap_tokenize(context);
296 /* try to find a matching sentence parser */
297 /* 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 */
298 /* binary search instead of linear (have to keep sentences in sorted order) O(NlogN) */
299 /* OR, when sentences are added, create a TRIE structure to find the names with a constant time search O(5) */
300 for(i=0;i<context->sentence_count;i++) {
301 s = &context->sentence[i];
303 if (strncmp(context->input_name,s->name,5) == 0) {
304 /* found a match, call its parser */
305 id = (*context->sentence[i].parser)(context,s);
318 +------+ +------+ +------+ +------+ +------+
319 | 0 |--$--> |1-hdr |--alnum--> |2-data|----\r-->| 6-LF |---\n--->| done |--> 0
320 +------+ +------+ +------+ +------+ +------+
322 * +--------\r-------+
324 +------+ +------+ +------+
325 |3-cks |--xdigit-->|4-cks |-xdigit->| 5-CR |
326 +------+ +------+ +------+
328 return to start conditions:
330 2. invalid character for state
333 two hex digits represent the XOR of all characters between, but not
334 including, the "$" and "*". A checksum is required on some
338 int nmeap_parse(nmeap_context_t *context,char ch)
342 /* check for input buffer overrun first to avoid duplicating code in the
345 if (context->input_count >= (sizeof(context->input)-1)) {
346 /* input buffer overrun, restart state machine */
347 context->input_state = 0;
348 /* reset input count */
349 context->input_count = 0;
353 context->input[context->input_count] = ch;
355 /* next buffer position */
356 context->input_count++;
358 /* run it through the lexical scanner */
359 switch(context->input_state) {
364 context->input_state = 1;
369 /* header error, start over */
371 context->input_state = 0;
372 context->input_count = 0;
375 /* LOOKING FOR 5 CHARACTER SENTENCE ID */
377 /* allow numbers even though it isn't usually done */
378 /* a proprietary id might have a numeral */
380 /* store name separately */
381 context->input_name[context->input_count - 2] = ch;
385 if (context->input_count >= 6) {
387 context->input_state = 2;
391 /* bad character, start over */
393 context->input_state = 0;
394 context->input_count = 0;
397 /* LOOKING FOR CR OR CHECKSUM INDICATOR */
400 /* this sentence has a checksum */
401 context->input_state = 3;
403 else if (ch == '\r') {
404 /* carriage return, no checksum, force a match */
407 context->input_state = 6;
410 /* continue accumulating data */
415 /* LOOKING FOR FIRST CHECKSUM CHARACTER */
417 /* must be upper case hex digit */
418 if (isxdigit(ch) && (ch <= 'F')) {
419 /* got first checksum byte */
420 context->input_state = 4;
421 context->icks = HEXTOBIN(ch) << 4;
424 /* input error, restart */
426 context->input_state = 0;
427 context->input_count = 0;
430 /* LOOKING FOR SECOND CHECKSUM CHARACTER */
432 /* must be upper case hex digit */
433 if (isxdigit(ch) && (ch <= 'F')) {
434 /* got second checksum byte */
435 context->input_state = 5;
436 context->icks += HEXTOBIN(ch);
439 /* input error, restart */
441 context->input_state = 0;
442 context->input_count = 0;
448 /* carriage return */
449 context->input_state = 6;
452 /* input error, restart */
454 context->input_state = 0;
455 context->input_count = 0;
458 /* LOOKING FOR LINE FEED */
461 /* linefeed, line complete */
464 context->input[context->input_count] = 0;
466 /* if the checksums match, process the sentence */
467 if (context->ccks == context->icks) {
469 status = nmeap_process(context);
471 /* count good messages */
475 /* count checksum errors */
479 /* restart next time */
480 context->input_state = 0;
481 context->input_count = 0;
484 /* input error, restart */
486 context->input_state = 0;
487 context->input_count = 0;
492 context->input_state = 0;
500 * parse a buffer of nmea data
502 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length)
512 /* for each byte in the buffer */
513 for(i=0;i<tlen;i++) {
514 /* decrement remaining byte count */
517 status = nmeap_parse(context,buffer[i]);
519 /* message found or error */
524 /* return remaining byte count */
531 * standard GPGGA sentence parser
533 int nmeap_gpgga(nmeap_context_t *context,nmeap_sentence_t *sentence)
539 /* get pointer to sentence data */
540 nmeap_gga_t *gga = (nmeap_gga_t *)sentence->data;
542 /* if there is a data element, extract data from the tokens */
544 gga->latitude = nmeap_latitude(context->token[2],context->token[3]);
545 gga->longitude = nmeap_longitude(context->token[4],context->token[5]);
546 gga->altitude = nmeap_altitude(context->token[9],context->token[10]);
547 gga->time = atoi(context->token[1]);
548 gga->satellites = atoi(context->token[7]);
549 gga->quality = atoi(context->token[6]);
550 gga->hdop = atof(context->token[8]);
551 gga->geoid = nmeap_altitude(context->token[11],context->token[12]);
555 /* print raw input string */
556 printf("%s",context->debug_input);
558 /* print some validation data */
559 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
561 /* print the tokens */
562 for(i=0;i<context->tokens;i++) {
563 printf("%d:%s\n",i,context->token[i]);
567 /* if the sentence has a callout, call it */
568 if (sentence->callout != 0) {
569 (*sentence->callout)(context,gga,context->user_data);
576 * standard GPRMCntence parser
578 int nmeap_gprmc(nmeap_context_t *context,nmeap_sentence_t *sentence)
584 /* get pointer to sentence data */
585 nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->data;
587 /* if there is a data element, use it */
589 /* extract data from the tokens */
590 rmc->time = atoi(context->token[1]);
591 rmc->warn = *context->token[2];
592 rmc->latitude = nmeap_latitude(context->token[3],context->token[4]);
593 rmc->longitude = nmeap_longitude(context->token[5],context->token[6]);
594 rmc->speed = atof(context->token[7]);
595 rmc->course = atof(context->token[8]);
596 rmc->date = atoi(context->token[9]);
597 rmc->magvar = atof(context->token[10]);
601 /* print raw input string */
602 printf("%s",context->debug_input);
604 /* print some validation data */
605 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
607 /* print the tokens */
608 for(i=0;i<context->tokens;i++) {
609 printf("%d:%s\n",i,context->token[i]);
613 /* if the sentence has a callout, call it */
614 if (sentence->callout != 0) {
615 (*sentence->callout)(context,rmc,context->user_data);