00001 /*
00002 Copyright (c) 2005, David M Howard (daveh at dmh2000.com)
00003 All rights reserved.
00004 
00005 This product is licensed for use and distribution under the BSD Open Source License.
00006 see the file COPYING for more details.
00007 
00008 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00009 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00010 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00011 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00012 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
00013 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00014 OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
00015 OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
00016 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00017 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
00018 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00019 
00020 */
00021
00029 #include <stdio.h>
00030 #include <stdlib.h>
00031 #include <string.h>
00032 #include <ctype.h>
00033
00034 #include "../inc/nmeap.h"
00035
00036 #include <cfg/debug.h>
00037
00038 #define assert(x)    ASSERT(x)
00039 
00040 #include "cfg/cfg_nmea.h"
00041
00042 #define LOG_LEVEL  NMEA_LOG_LEVEL
00043 #define LOG_FORMAT NMEA_LOG_FORMAT
00044 #include <cfg/log.h>
00045
00046 #ifdef _DEBUG
00047     #undef NDEBUG
00048     #define printf(str,...)  LOG_INFO(str, ## __VA_ARGS__)
00049 #endif
00050 
00051 /* this only works if you are sure you have an upper case hex digit */
00052 #define HEXTOBIN(ch) ((ch <= '9') ? ch - '0' : ch - ('A' - 10))
00053 
00054 /* forward references */
00055 int nmeap_init(nmeap_context_t *context,void *user_data);
00056 int nmeap_addParser(nmeap_context_t         *context,
00057                      const char             *sentence_name,
00058                      nmeap_sentence_parser_t sentence_parser,
00059                      nmeap_callout_t         sentence_callout,
00060                      void                  *sentence_data
00061                      );
00062 int nmeap_tokenize(nmeap_context_t *context);
00063 int nmeap_process(nmeap_context_t *context);
00064 int nmeap_parse(nmeap_context_t *context,char ch);
00065 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length);
00066
00070 double nmeap_latitude(const char *plat,const char *phem)
00071 {
00072     double lat;
00073     int    deg;
00074     double min;
00075     int    ns;
00076
00077     assert(plat != 0);
00078     assert(phem != 0);
00079
00080     if (*plat == 0) {
00081         return 0.0;
00082     }
00083     if (*phem == 0) {
00084         return 0.0;
00085     }
00086
00087     /* north lat is +, south lat is - */
00088     if (*phem == 'N') {
00089         ns = 1;
00090     }
00091     else {
00092         ns = -1;
00093     }
00094
00095     /* latitude is degrees, minutes, fractional minutes */
00096     /* no validation is performed on the token. it better be good.*/
00097     /* if it comes back 0.0 then probably the token was bad */
00098     lat = atof(plat);
00099
00100     /* extract the degree part */
00101     deg = (int)(lat / 100.0);
00102
00103     /* mask out the degrees */
00104     min = lat - (deg * 100.0);
00105
00106     /* compute the actual latitude in degrees.decimal-degrees */
00107     lat = (deg + (min / 60.0)) * ns;
00108
00109     return lat;
00110 }
00111
00115 double nmeap_longitude(const char *plon,const char *phem)
00116 {
00117     double lon;
00118     int    deg;
00119     double min;
00120     int    ew;
00121
00122     assert(plon != 0);
00123     assert(phem != 0);
00124
00125     if (*plon == 0) {
00126         return 0.0;
00127     }
00128     if (*phem == 0) {
00129         return 0.0;
00130     }
00131
00132     /* west long is negative, east long is positive */
00133     if (*phem == 'E') {
00134         ew = 1;
00135     }
00136     else {
00137         ew = -1;
00138     }
00139
00140     /* longitude is degrees, minutes, fractional minutes */
00141     /* no validation is performed on the token. it better be good.*/
00142     /* if it comes back 0.0 then probably the token was bad */
00143     lon = atof(plon);
00144
00145     /* extract the degree part */
00146     deg = (int)(lon / 100.0);
00147
00148     /* mask out the degrees */
00149     min = lon - (deg * 100.0);
00150
00151     /* compute the actual lonitude in degrees.decimal-degrees */
00152     lon = (deg + (min / 60.0)) * ew;
00153
00154
00155     return lon;
00156 }
00157
00162 double nmeap_altitude(const char *palt,const char *punits)
00163 {
00164     double alt;
00165
00166     if (*palt == 0) {
00167         return 0.0;
00168     }
00169
00170     /* convert with no error checking */
00171     alt = atof(palt);
00172
00173     if (*punits == 'M') {
00174         /* already in meters */
00175     }
00176     else if (*punits == 'F') {
00177         /* convert to feet */
00178         alt = alt * 3.2808399;
00179     }
00180
00181     return alt;
00182 }
00183
00187 int nmeap_init(nmeap_context_t *context,void *user_data)
00188 {
00189     assert(context != 0);
00190
00191     memset(context,0,sizeof(*context));
00192
00193     context->user_data = user_data;
00194
00195     return 0;
00196 }
00197
00201 int nmeap_addParser(nmeap_context_t         *context,
00202                      const char             *sentence_name,
00203                      nmeap_sentence_parser_t sentence_parser,
00204                      nmeap_callout_t         sentence_callout,
00205                      void                  *sentence_data
00206                      )
00207 {
00208     nmeap_sentence_t *s = 0;
00209
00210     /* runtime error */
00211     assert(context != 0);
00212
00213     /* sentence capacity overflow */
00214     if (context->sentence_count >= NMEAP_MAX_SENTENCES) {
00215         return -1;
00216     }
00217
00218     /* point at next empty sentence buffer */
00219     s = &context->sentence[context->sentence_count];
00220
00221     /* advance sentence data count */
00222     context->sentence_count++;
00223
00224     /* clear the sentence data */
00225     memset(s,0,sizeof(*s));
00226
00227     /* name */
00228     strncpy(s->name,sentence_name,NMEAP_MAX_SENTENCE_NAME_LENGTH);
00229
00230     /* parser */
00231     s->parser = sentence_parser;
00232
00233     /* callout */
00234     s->callout = sentence_callout;
00235
00236     /* data */
00237     s->data    = sentence_data;
00238
00239     return 0;
00240 }
00241
00245 int nmeap_tokenize(nmeap_context_t *context)
00246 {
00247     char *s;
00248     int   tokens;
00249     int   state;
00250
00251     /* first token is header. assume it is there */
00252     tokens = 0;
00253     s = context->input;
00254     context->token[tokens] = s;
00255
00256     /* get rest of tokens */
00257     tokens = 1;
00258     state = 0;
00259     while((*s != 0)&&(tokens < NMEAP_MAX_TOKENS)) {
00260         switch(state) {
00261         case 0:
00262             /* looking for end of a token */
00263             if (*s == ',') {
00264                 /* delimit at the comma */
00265                 *s    = 0;
00266                 /* new token */
00267                 state = 1;
00268             }
00269             break;
00270         case 1:
00271             /* start of next token, might be another comma */
00272             context->token[tokens++] = s;
00273             if (*s == ',') {
00274                 /* delimit at the comma */
00275                 *s    = 0;
00276             }
00277             else {
00278                 /* not a comma */
00279                 state = 0;
00280             }
00281             break;
00282         default:
00283             state = 0;
00284             break;
00285         }
00286
00287         // next character
00288         s++;
00289     }
00290     return tokens;
00291 }
00292
00296 int nmeap_process(nmeap_context_t *context)
00297 {
00298     int id = 0;
00299     int i;
00300     nmeap_sentence_t *s;
00301
00302     /* copy the input to a debug buffer */
00303     /* remove debug_input when everything is working. */
00304     strncpy(context->debug_input,context->input,sizeof(context->debug_input));
00305
00306     /* tokenize the input */
00307     context->tokens = nmeap_tokenize(context);
00308
00309     /* try to find a matching sentence parser */
00310     /* 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 */
00311     /* binary search instead of linear (have to keep sentences in sorted order) O(NlogN) */
00312     /* OR, when sentences are added, create a TRIE structure to find the names with a constant time search O(5) */
00313     for(i=0;i<context->sentence_count;i++) {
00314         s = &context->sentence[i];
00315         assert(s != 0);
00316         if (strncmp(context->input_name,s->name,5) == 0) {
00317             /* found a match, call its parser */
00318             id = (*context->sentence[i].parser)(context,s);
00319             if (id > 0) {
00320                 break;
00321             }
00322         }
00323     }
00324
00325     return id;
00326 }
00327
00351 int nmeap_parse(nmeap_context_t *context,char ch)
00352 {
00353     int status = 0;
00354
00355     /* check for input buffer overrun first to avoid duplicating code in the
00356     individual states
00357     */
00358     if ((size_t)context->input_count >= (sizeof(context->input)-1)) {
00359         /* input buffer overrun, restart state machine */
00360         context->input_state = 0;
00361         /* reset input count */
00362         context->input_count = 0;
00363     }
00364
00365     /* store the byte */
00366     context->input[context->input_count] = ch;
00367
00368     /* next buffer position */
00369     context->input_count++;
00370
00371     /* run it through the lexical scanner */
00372     switch(context->input_state) {
00373     /* LOOKING FOR $ */
00374     case 0:
00375         if (ch == '$') {
00376             /*look for id */
00377             context->input_state = 1;
00378             context->ccks        = 0;
00379             context->icks        = 0;
00380         }
00381         else {
00382             /* header error, start over */
00383             context->err_hdr++;
00384             context->input_state = 0;
00385             context->input_count = 0;
00386         }
00387         break;
00388     /* LOOKING FOR 5 CHARACTER SENTENCE ID */
00389     case 1:
00390         /* allow numbers even though it isn't usually done */
00391         /* a proprietary id might have a numeral */
00392         if (isalnum((unsigned char)ch)) {
00393             /* store name separately */
00394             context->input_name[context->input_count - 2] = ch;
00395             /* checksum */
00396             context->ccks ^= ch;
00397             /* end of header? */
00398             if (context->input_count >= 6) {
00399                 /* yes, get body */
00400                 context->input_state = 2;
00401             }
00402         }
00403         else {
00404             /* bad character, start over */
00405             context->err_id++;
00406             context->input_state = 0;
00407             context->input_count = 0;
00408         }
00409         break;
00410     /* LOOKING FOR CR OR CHECKSUM INDICATOR */
00411     case 2:
00412         if (ch == '*') {
00413             /* this sentence has a checksum */
00414             context->input_state = 3;
00415         }
00416         else if (ch == '\r') {
00417             /* carriage return, no checksum, force a match */
00418             context->icks = 0;
00419             context->ccks = 0;
00420             context->input_state = 6;
00421         }
00422         else {
00423             /* continue accumulating data */
00424             /* checksum */
00425             context->ccks ^= ch;
00426         }
00427         break;
00428     /* LOOKING FOR FIRST CHECKSUM CHARACTER */
00429     case 3:
00430         /* must be upper case hex digit */
00431         if (isxdigit((unsigned char)ch) && (ch <= 'F')) {
00432             /* got first checksum byte */
00433             context->input_state = 4;
00434             context->icks = HEXTOBIN(ch) << 4;
00435         }
00436         else {
00437             /* input error, restart */
00438             context->err_cks++;
00439             context->input_state = 0;
00440             context->input_count = 0;
00441         }
00442         break;
00443         /* LOOKING FOR SECOND CHECKSUM CHARACTER */
00444     case 4:
00445         /* must be upper case hex digit */
00446         if (isxdigit((unsigned char)ch) && (ch <= 'F')) {
00447             /* got second checksum byte */
00448             context->input_state = 5;
00449             context->icks += HEXTOBIN(ch);
00450         }
00451         else {
00452             /* input error, restart */
00453             context->err_cks++;
00454             context->input_state = 0;
00455             context->input_count = 0;
00456         }
00457         break;
00458     /* LOOKING FOR CR */
00459     case 5:
00460         if (ch == '\r') {
00461             /* carriage return */
00462             context->input_state = 6;
00463         }
00464         else {
00465             /* input error, restart */
00466             context->err_crl++;
00467             context->input_state = 0;
00468             context->input_count = 0;
00469         }
00470         break;
00471     /* LOOKING FOR LINE FEED */
00472     case 6:
00473         if (ch == '\n') {
00474             /* linefeed, line complete */
00475
00476             /* delimit buffer */
00477             context->input[context->input_count] = 0;
00478
00479             /* if the checksums match, process the sentence */
00480             if (context->ccks == context->icks) {
00481                 /* process */
00482                 status = nmeap_process(context);
00483
00484                 /* count good messages */
00485                 context->msgs++;
00486             }
00487             else {
00488                 /* count checksum errors */
00489                 context->err_cks++;
00490             }
00491
00492             /* restart next time */
00493             context->input_state = 0;
00494             context->input_count = 0;
00495         }
00496         else {
00497             /* input error, restart */
00498             context->err_crl++;
00499             context->input_state = 0;
00500             context->input_count = 0;
00501         }
00502         break;
00503     default:
00504         context->err_unk++;
00505         context->input_state = 0;
00506         break;
00507     }
00508
00509     return status;
00510 }
00511
00515 int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length)
00516 {
00517     int  i;
00518     int  status;
00519     int  rem;
00520     int  tlen;
00521
00522     tlen   = *length;
00523     rem    = *length;
00524     status = 0;
00525     /* for each byte in the buffer */
00526     for(i=0;i<tlen;i++) {
00527         /* decrement remaining byte count */
00528         rem--;
00529         /* parse the byte */
00530         status = nmeap_parse(context,buffer[i]);
00531         if (status != 0) {
00532             /* message found or error */
00533             break;
00534         }
00535     }
00536
00537     /* return remaining byte count */
00538     *length = rem;
00539
00540     return status;
00541 }
00542
00546 int nmeap_gpgga(nmeap_context_t *context,nmeap_sentence_t *sentence)
00547 {
00548 #ifndef NDEBUG
00549     int i;
00550 #endif
00551 
00552     /* get pointer to sentence data */
00553     nmeap_gga_t *gga = (nmeap_gga_t *)sentence->data;
00554
00555     /* if there is a data element, extract data from the tokens */
00556     if (gga != 0) {
00557         gga->latitude  = nmeap_latitude(context->token[2],context->token[3]);
00558         gga->longitude = nmeap_longitude(context->token[4],context->token[5]);
00559         gga->altitude  = nmeap_altitude(context->token[9],context->token[10]);
00560         gga->time       = atoi(context->token[1]);
00561         gga->satellites = atoi(context->token[7]);
00562         gga->quality    = atoi(context->token[6]);
00563         gga->hdop       = atof(context->token[8]);
00564         gga->geoid      = nmeap_altitude(context->token[11],context->token[12]);
00565     }
00566
00567 #ifndef NDEBUG
00568     /* print raw input string */
00569     printf("%s",context->debug_input);
00570
00571     /* print some validation data */
00572     printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
00573
00574     /* print the tokens */
00575     for(i=0;i<context->tokens;i++) {
00576         printf("%d:%s\n",i,context->token[i]);
00577     }
00578 #endif
00579 
00580     /* if the sentence has a callout, call it */
00581     if (sentence->callout != 0) {
00582         (*sentence->callout)(context,gga,context->user_data);
00583     }
00584
00585     return NMEAP_GPGGA;
00586 }
00587
00591 int nmeap_gprmc(nmeap_context_t *context,nmeap_sentence_t *sentence)
00592 {
00593 #ifndef NDEBUG
00594     int i;
00595 #endif
00596 
00597     /* get pointer to sentence data */
00598     nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->data;
00599
00600     /* if there is a data element, use it */
00601     if (rmc != 0) {
00602         /* extract data from the tokens */
00603         rmc->time       = atoi(context->token[1]);
00604         rmc->warn       = *context->token[2];
00605         rmc->latitude  = nmeap_latitude(context->token[3],context->token[4]);
00606         rmc->longitude = nmeap_longitude(context->token[5],context->token[6]);
00607         rmc->speed      = atof(context->token[7]);
00608         rmc->course     = atof(context->token[8]);
00609         rmc->date       = atoi(context->token[9]);
00610         rmc->magvar     = atof(context->token[10]);
00611     }
00612
00613 #ifndef NDEBUG
00614     /* print raw input string */
00615     printf("%s",context->debug_input);
00616
00617     /* print some validation data */
00618     printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
00619
00620     /* print the tokens */
00621     for(i=0;i<context->tokens;i++) {
00622         printf("%d:%s\n",i,context->token[i]);
00623     }
00624 #endif
00625 
00626     /* if the sentence has a callout, call it */
00627     if (sentence->callout != 0) {
00628         (*sentence->callout)(context,rmc,context->user_data);
00629     }
00630
00631     return NMEAP_GPRMC;
00632 }
00633
00634