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