BeRTOS
|
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