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