BeRTOS
nmeap01.c
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