tok.c

00001 /*
00002  *
00003  * NMEA library
00004  * URL: http://nmea.sourceforge.net
00005  * Author: Tim (xtimor@gmail.com)
00006  * Licence: http://www.gnu.org/licenses/lgpl.html
00007  * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $
00008  *
00009  */
00010 
00011 /*! \file tok.h */
00012 
00013 #include "nmea/tok.h "
00014 
00015 #include <stdarg.h>
00016 #include <stdlib.h>
00017 #include <stdio.h>
00018 #include <ctype.h>
00019 #include <string.h>
00020 #include <limits.h>
00021 
00022 #define NMEA_TOKS_COMPARE   (1)
00023 #define NMEA_TOKS_PERCENT   (2)
00024 #define NMEA_TOKS_WIDTH     (3)
00025 #define NMEA_TOKS_TYPE      (4)
00026 
00027 /**
00028  * \brief Calculate control sum of binary buffer
00029  */
00030 int nmea_calc_crc(const char *buff, int buff_sz)
00031 {
00032     int chsum = 0,
00033         it;
00034 
00035     for(it = 0; it < buff_sz; ++it)
00036         chsum ^= (int)buff[it];
00037 
00038     return chsum;
00039 }
00040 
00041 /**
00042  * \brief Convert string to number
00043  */
00044 int nmea_atoi(const char *str, int str_sz, int radix)
00045 {
00046     char *tmp_ptr;
00047     char buff[NMEA_CONVSTR_BUF];
00048     int res = 0;
00049 
00050     if(str_sz < NMEA_CONVSTR_BUF)
00051     {
00052         memcpy(&buff[0], str, str_sz);
00053         buff[str_sz] = '\0';
00054         res = strtol(&buff[0], &tmp_ptr, radix);
00055     }
00056 
00057     return res;
00058 }
00059 
00060 /**
00061  * \brief Convert string to fraction number
00062  */
00063 double nmea_atof(const char *str, int str_sz)
00064 {
00065     char *tmp_ptr;
00066     char buff[NMEA_CONVSTR_BUF];
00067     double res = 0;
00068 
00069     if(str_sz < NMEA_CONVSTR_BUF)
00070     {
00071         memcpy(&buff[0], str, str_sz);
00072         buff[str_sz] = '\0';
00073         res = strtod(&buff[0], &tmp_ptr);
00074     }
00075 
00076     return res;
00077 }
00078 
00079 /**
00080  * \brief Formating string (like standart printf) with CRC tail (*CRC)
00081  */
00082 int nmea_printf(char *buff, int buff_sz, const char *format, ...)
00083 {
00084     int retval, add = 0;
00085     va_list arg_ptr;
00086 
00087     if(buff_sz <= 0)
00088         return 0;
00089 
00090     va_start(arg_ptr, format);
00091 
00092     retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr);
00093 
00094     if(retval > 0)
00095     {
00096         add = NMEA_POSIX(snprintf)(
00097             buff + retval, buff_sz - retval, "*%02x\r\n",
00098             nmea_calc_crc(buff + 1, retval - 1));
00099     }
00100 
00101     retval += add;
00102 
00103     if(retval < 0 || retval > buff_sz)
00104     {
00105         memset(buff, ' ', buff_sz);
00106         retval = buff_sz;
00107     }
00108 
00109     va_end(arg_ptr);
00110 
00111     return retval;
00112 }
00113 
00114 /**
00115  * \brief Analyse string (specificate for NMEA sentences)
00116  */
00117 int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
00118 {
00119     const char *beg_tok;
00120     const char *end_buf = buff + buff_sz;
00121 
00122     va_list arg_ptr;
00123     int tok_type = NMEA_TOKS_COMPARE;
00124     int width = 0;
00125     const char *beg_fmt = 0;
00126     int snum = 0, unum = 0;
00127 
00128     int tok_count = 0;
00129     void *parg_target;
00130 
00131     va_start(arg_ptr, format);
00132     
00133     for(; *format && buff < end_buf; ++format)
00134     {
00135         switch(tok_type)
00136         {
00137         case NMEA_TOKS_COMPARE:
00138             if('%' == *format)
00139                 tok_type = NMEA_TOKS_PERCENT;
00140             else if(*buff++ != *format)
00141                 goto fail;
00142             break;
00143         case NMEA_TOKS_PERCENT:
00144             width = 0;
00145             beg_fmt = format;
00146             tok_type = NMEA_TOKS_WIDTH;
00147         case NMEA_TOKS_WIDTH:
00148             if(isdigit(*format))
00149                 break;
00150             {
00151                 tok_type = NMEA_TOKS_TYPE;
00152                 if(format > beg_fmt)
00153                     width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
00154             }
00155         case NMEA_TOKS_TYPE:
00156             beg_tok = buff;
00157 
00158             if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
00159                 width = 1;
00160 
00161             if(width)
00162             {
00163                 if(buff + width <= end_buf)
00164                     buff += width;
00165                 else
00166                     goto fail;
00167             }
00168             else
00169             {
00170                 if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
00171                     buff = end_buf;
00172             }
00173 
00174             if(buff > end_buf)
00175                 goto fail;
00176 
00177             tok_type = NMEA_TOKS_COMPARE;
00178             tok_count++;
00179 
00180             parg_target = 0; width = (int)(buff - beg_tok);
00181 
00182             switch(*format)
00183             {
00184             case 'c':
00185             case 'C':
00186                 parg_target = (void *)va_arg(arg_ptr, char *);
00187                 if(width && 0 != (parg_target))
00188                     *((char *)parg_target) = *beg_tok;
00189                 break;
00190             case 's':
00191             case 'S':
00192                 parg_target = (void *)va_arg(arg_ptr, char *);
00193                 if(width && 0 != (parg_target))
00194                 {
00195                     memcpy(parg_target, beg_tok, width);
00196                     ((char *)parg_target)[width] = '\0';
00197                 }
00198                 break;
00199             case 'f':
00200             case 'g':
00201             case 'G':
00202             case 'e':
00203             case 'E':
00204                 parg_target = (void *)va_arg(arg_ptr, double *);
00205                 if(width && 0 != (parg_target))
00206                     *((double *)parg_target) = nmea_atof(beg_tok, width);
00207                 break;
00208             };
00209 
00210             if(parg_target)
00211                 break;
00212             if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
00213                 break;
00214             if(!width)
00215                 break;
00216 
00217             switch(*format)
00218             {
00219             case 'd':
00220             case 'i':
00221                 snum = nmea_atoi(beg_tok, width, 10);
00222                 memcpy(parg_target, &snum, sizeof(int));
00223                 break;
00224             case 'u':
00225                 unum = nmea_atoi(beg_tok, width, 10);
00226                 memcpy(parg_target, &unum, sizeof(unsigned int));
00227                 break;
00228             case 'x':
00229             case 'X':
00230                 unum = nmea_atoi(beg_tok, width, 16);
00231                 memcpy(parg_target, &unum, sizeof(unsigned int));
00232                 break;
00233             case 'o':
00234                 unum = nmea_atoi(beg_tok, width, 8);
00235                 memcpy(parg_target, &unum, sizeof(unsigned int));
00236                 break;
00237             default:
00238                 goto fail;
00239             };
00240 
00241             break;
00242         };
00243     }
00244 
00245 fail:
00246 
00247     va_end(arg_ptr);
00248 
00249     return tok_count;
00250 }