bertos/net/nmeap/src/nmeap01.c

   1 /*
   2 Copyright (c) 2005, David M Howard (daveh at dmh2000.com)
   3 All rights reserved.
   4
   5 This product is licensed for use and distribution under the BSD Open Source License.
   6 see the file COPYING for more details.
   7
   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.
  19
  20 */
  21
  22 /**
  23  * nmeap01.c
  24  * nmeap gps data parser
  25  *
  26  * see the file COPYING for terms of the licnese
  27 */
  28
  29
  30 #include <stdio.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <assert.h>
  34 #include <ctype.h>
  35 #include "nmeap.h"
  36
  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))
  39
  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,
  46                      void                  *sentence_data
  47                                          );
  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);
  52
  53 /**
  54  * get a latitude out of a pair of nmea tokens
  55  */
  56 double nmeap_latitude(const char *plat,const char *phem)
  57 {
  58     double lat;
  59     int    deg;
  60     double min;
  61     int    ns;
  62
  63     assert(plat != 0);
  64     assert(phem != 0);
  65
  66     if (*plat == 0) {
  67         return 0.0;
  68     }
  69     if (*phem == 0) {
  70         return 0.0;
  71     }
  72
  73     /* north lat is +, south lat is - */
  74     if (*phem == 'N') {
  75         ns = 1;
  76     }
  77     else {
  78         ns = -1;
  79     }
  80
  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 */
  84     lat = atof(plat);
  85
  86     /* extract the degree part */
  87     deg = (int)(lat / 100.0);
  88
  89     /* mask out the degrees */
  90     min = lat - (deg * 100.0);
  91
  92     /* compute the actual latitude in degrees.decimal-degrees */
  93     lat = (deg + (min / 60.0)) * ns;
  94
  95     return lat;
  96 }
  97
  98 /**
  99  * get a longitude out of a pair of nmea tokens
 100  */
 101 double nmeap_longitude(const char *plon,const char *phem)
 102 {
 103     double lon;
 104     int    deg;
 105     double min;
 106     int    ew;
 107
 108     assert(plon != 0);
 109     assert(phem != 0);
 110
 111     if (*plon == 0) {
 112         return 0.0;
 113     }
 114     if (*phem == 0) {
 115         return 0.0;
 116     }
 117
 118     /* west long is negative, east long is positive */
 119     if (*phem == 'E') {
 120         ew = 1;
 121     }
 122     else {
 123         ew = -1;
 124     }
 125
 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 */
 129     lon = atof(plon);
 130
 131     /* extract the degree part */
 132     deg = (int)(lon / 100.0);
 133
 134     /* mask out the degrees */
 135     min = lon - (deg * 100.0);
 136
 137     /* compute the actual lonitude in degrees.decimal-degrees */
 138     lon = (deg + (min / 60.0)) * ew;
 139
 140
 141     return lon;
 142 }
 143
 144 /**
 145  * get an altitude longitude out of a pair of nmea tokens
 146  * ALTITUDE is returned in METERS
 147  */
 148 double nmeap_altitude(const char *palt,const char *punits)
 149 {
 150     double alt;
 151
 152     if (*palt == 0) {
 153         return 0.0;
 154     }
 155
 156     /* convert with no error checking */
 157     alt = atof(palt);
 158
 159     if (*punits == 'M') {
 160         /* already in meters */
 161     }
 162     else if (*punits == 'F') {
 163         /* convert to feet */
 164         alt = alt * 3.2808399;
 165     }
 166
 167     return alt;
 168 }
 169
 170 /**
 171  * initialize an NMEA parser
 172  */
 173 int nmeap_init(nmeap_context_t *context,void *user_data)
 174 {
 175     assert(context != 0);
 176
 177     memset(context,0,sizeof(*context));
 178
 179     context->user_data = user_data;
 180
 181     return 0;
 182 }
 183
 184
 185 /**
 186  * register an NMEA sentence parser
 187  */
 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,
 192                      void                  *sentence_data
 193                                          )
 194 {
 195     nmeap_sentence_t *s = 0;
 196
 197     /* runtime error */
 198     assert(context != 0);
 199
 200     /* sentence capacity overflow */
 201     if (context->sentence_count >= NMEAP_MAX_SENTENCES) {
 202         return -1;
 203     }
 204
 205     /* point at next empty sentence buffer */
 206     s = &context->sentence[context->sentence_count];
 207
 208     /* advance sentence data count */
 209     context->sentence_count++;
 210
 211     /* clear the sentence data */
 212     memset(s,0,sizeof(*s));
 213
 214     /* name */
 215     strncpy(s->name,sentence_name,NMEAP_MAX_SENTENCE_NAME_LENGTH);
 216
 217     /* parser */
 218     s->parser = sentence_parser;
 219
 220     /* callout */
 221     s->callout = sentence_callout;
 222
 223     /* data */
 224     s->data    = sentence_data;
 225
 226     return 0;
 227 }
 228
 229 /**
 230  * tokenize a buffer
 231  */
 232 int nmeap_tokenize(nmeap_context_t *context)
 233 {
 234     char *s;
 235     int   tokens;
 236     int   state;
 237
 238     /* first token is header. assume it is there */
 239     tokens = 0;
 240     s = context->input;
 241     context->token[tokens] = s;
 242
 243     /* get rest of tokens */
 244     tokens = 1;
 245     state = 0;
 246     while((*s != 0)&&(tokens < NMEAP_MAX_TOKENS)) {
 247         switch(state) {
 248         case 0:
 249             /* looking for end of a token */
 250             if (*s == ',') {
 251                 /* delimit at the comma */
 252                 *s    = 0;
 253                 /* new token */
 254                 state = 1;
 255             }
 256             break;
 257         case 1:
 258             /* start of next token, might be another comma */
 259             context->token[tokens++] = s;
 260             if (*s == ',') {
 261                 /* delimit at the comma */
 262                 *s    = 0;
 263             }
 264             else {
 265                 /* not a comma */
 266                 state = 0;
 267             }
 268             break;
 269         default:
 270             state = 0;
 271             break;
 272         }
 273
 274         // next character
 275         s++;
 276     }
 277     return tokens;
 278 }
 279
 280 /**
 281  * process a sentence
 282  */
 283 int nmeap_process(nmeap_context_t *context)
 284 {
 285     int id;
 286     int i;
 287     nmeap_sentence_t *s;
 288
 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));
 292
 293     /* tokenize the input */
 294     context->tokens = nmeap_tokenize(context);
 295
 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];
 302         assert(s != 0);
 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);
 306             if (id > 0) {
 307                 break;
 308             }
 309         }
 310     }
 311
 312     return id;
 313 }
 314
 315 /**
 316                  +-5-+              +---+
 317                  v   |              v   |
 318  +------+       +------+           +------+         +------+         +------+
 319  |   0  |--$--> |1-hdr |--alnum--> |2-data|----\r-->| 6-LF |---\n--->| done |--> 0
 320  +------+       +------+           +------+         +------+         +------+
 321                                     |                 ^
 322                                     *                 +--------\r-------+
 323                                     V                                   |
 324                                  +------+           +------+         +------+
 325                                  |3-cks |--xdigit-->|4-cks |-xdigit->| 5-CR |
 326                                  +------+           +------+         +------+
 327
 328 return to start conditions:
 329 1. buffer overflow
 330 2. invalid character for state
 331
 332 checksum calculation
 333 two hex digits represent the XOR of all characters between, but not
 334 including, the "$" and "*".  A checksum is required on some
 335 sentences.
 336
 337 */
 338 int nmeap_parse(nmeap_context_t *context,char ch)
 339 {
 340     int status = 0;
 341
 342     /* check for input buffer overrun first to avoid duplicating code in the
 343     individual states
 344     */
 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;
 350     }
 351
 352     /* store the byte */
 353     context->input[context->input_count] = ch;
 354
 355     /* next buffer position */
 356     context->input_count++;
 357
 358     /* run it through the lexical scanner */
 359     switch(context->input_state) {
 360     /* LOOKING FOR $ */
 361     case 0:
 362         if (ch == '$') {
 363             /*look for id */
 364             context->input_state = 1;
 365             context->ccks        = 0;
 366                         context->icks        = 0;
 367         }
 368         else {
 369             /* header error, start over */
 370             context->err_hdr++;
 371             context->input_state = 0;
 372             context->input_count = 0;
 373         }
 374         break;
 375     /* LOOKING FOR 5 CHARACTER SENTENCE ID */
 376     case 1:
 377         /* allow numbers even though it isn't usually done */
 378         /* a proprietary id might have a numeral */
 379         if (isalnum(ch)) {
 380             /* store name separately */
 381             context->input_name[context->input_count - 2] = ch;
 382             /* checksum */
 383             context->ccks ^= ch;
 384             /* end of header? */
 385             if (context->input_count >= 6) {
 386                 /* yes, get body */
 387                 context->input_state = 2;
 388             }
 389         }
 390         else {
 391             /* bad character, start over */
 392             context->err_id++;
 393             context->input_state = 0;
 394             context->input_count = 0;
 395         }
 396         break;
 397     /* LOOKING FOR CR OR CHECKSUM INDICATOR */
 398     case 2:
 399         if (ch == '*') {
 400             /* this sentence has a checksum */
 401             context->input_state = 3;
 402         }
 403         else if (ch == '\r') {
 404             /* carriage return, no checksum, force a match */
 405                         context->icks = 0;
 406                         context->ccks = 0;
 407             context->input_state = 6;
 408         }
 409         else {
 410             /* continue accumulating data */
 411             /* checksum */
 412             context->ccks ^= ch;
 413         }
 414         break;
 415     /* LOOKING FOR FIRST CHECKSUM CHARACTER */
 416     case 3:
 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;
 422         }
 423         else {
 424             /* input error, restart */
 425             context->err_cks++;
 426             context->input_state = 0;
 427             context->input_count = 0;
 428         }
 429         break;
 430         /* LOOKING FOR SECOND CHECKSUM CHARACTER */
 431     case 4:
 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);
 437         }
 438         else {
 439             /* input error, restart */
 440             context->err_cks++;
 441             context->input_state = 0;
 442             context->input_count = 0;
 443         }
 444         break;
 445     /* LOOKING FOR CR */
 446     case 5:
 447         if (ch == '\r') {
 448             /* carriage return */
 449             context->input_state = 6;
 450         }
 451         else {
 452             /* input error, restart */
 453             context->err_crl++;
 454             context->input_state = 0;
 455             context->input_count = 0;
 456         }
 457         break;
 458     /* LOOKING FOR LINE FEED */
 459     case 6:
 460         if (ch == '\n') {
 461             /* linefeed, line complete */
 462
 463             /* delimit buffer */
 464             context->input[context->input_count] = 0;
 465
 466             /* if the checksums match, process the sentence */
 467                         if (context->ccks == context->icks) {
 468                                 /* process */
 469                                 status = nmeap_process(context);
 470
 471                                 /* count good messages */
 472                                 context->msgs++;
 473                         }
 474                         else {
 475                                 /* count checksum errors */
 476                                 context->err_cks++;
 477                         }
 478
 479             /* restart next time */
 480             context->input_state = 0;
 481             context->input_count = 0;
 482         }
 483         else {
 484             /* input error, restart */
 485             context->err_crl++;
 486             context->input_state = 0;
 487             context->input_count = 0;
 488         }
 489         break;
 490     default:
 491         context->err_unk++;
 492         context->input_state = 0;
 493         break;
 494     }
 495
 496         return status;
 497 }
 498
 499 /**
 500  * parse a buffer of nmea data
 501  */
 502 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length)
 503 {
 504     int  i;
 505     int  status;
 506     int  rem;
 507     int  tlen;
 508
 509     tlen   = *length;
 510     rem    = *length;
 511     status = 0;
 512     /* for each byte in the buffer */
 513     for(i=0;i<tlen;i++) {
 514         /* decrement remaining byte count */
 515         rem--;
 516         /* parse the byte */
 517         status = nmeap_parse(context,buffer[i]);
 518         if (status != 0) {
 519             /* message found or error */
 520             break;
 521         }
 522     }
 523
 524     /* return remaining byte count */
 525     *length = rem;
 526
 527     return status;
 528 }
 529
 530 /**
 531  * standard GPGGA sentence parser
 532  */
 533 int nmeap_gpgga(nmeap_context_t *context,nmeap_sentence_t *sentence)
 534 {
 535 #ifndef NDEBUG
 536     int i;
 537 #endif
 538
 539     /* get pointer to sentence data */
 540     nmeap_gga_t *gga = (nmeap_gga_t *)sentence->data;
 541
 542     /* if there is a data element, extract data from the tokens */
 543         if (gga != 0) {
 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]);
 552         }
 553
 554 #ifndef NDEBUG
 555     /* print raw input string */
 556     printf("%s",context->debug_input);
 557
 558     /* print some validation data */
 559     printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
 560
 561     /* print the tokens */
 562     for(i=0;i<context->tokens;i++) {
 563         printf("%d:%s\n",i,context->token[i]);
 564     }
 565 #endif
 566
 567     /* if the sentence has a callout, call it */
 568     if (sentence->callout != 0) {
 569         (*sentence->callout)(context,gga,context->user_data);
 570     }
 571
 572     return NMEAP_GPGGA;
 573 }
 574
 575 /**
 576  * standard GPRMCntence parser
 577  */
 578 int nmeap_gprmc(nmeap_context_t *context,nmeap_sentence_t *sentence)
 579 {
 580 #ifndef NDEBUG
 581     int i;
 582 #endif
 583
 584     /* get pointer to sentence data */
 585     nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->data;
 586
 587         /* if there is a data element, use it */
 588         if (rmc != 0) {
 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]);
 598         }
 599
 600 #ifndef NDEBUG
 601     /* print raw input string */
 602     printf("%s",context->debug_input);
 603
 604     /* print some validation data */
 605     printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
 606
 607     /* print the tokens */
 608     for(i=0;i<context->tokens;i++) {
 609         printf("%d:%s\n",i,context->token[i]);
 610     }
 611 #endif
 612
 613     /* if the sentence has a callout, call it */
 614     if (sentence->callout != 0) {
 615         (*sentence->callout)(context,rmc,context->user_data);
 616     }
 617
 618     return NMEAP_GPRMC;
 619 }
 620
 621