Guido Ottaviani
A porting of a GPS decoding and presenting program within the mbos RTOS. It is not a definitive application but a study program to test NMEA full decoding library and a first approach to an RTOS. Many thanks to Andrew Levido for his support and his patience on teaching me the RTOS principles from the other side of the Earth. It uses NMEA library by Tim (xtimor@gmail.com) ported by Ken Todotani (http://mbed.org/users/todotani/) on public mbed library (http://mbed.org/users/todotani/programs/GPS_nmeaLib/5yo4h) also available, as original universal C library, on http://nmea.sourceforge.net
Dependencies: mbos Watchdog TextLCD mbed ConfigFile
nmea/tok.c@0:d177c0087d1f, 2012-01-29 (annotated)
- Committer:
- guiott
- Date:
- Sun Jan 29 16:06:12 2012 +0000
- Revision:
- 0:d177c0087d1f
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| guiott | 0:d177c0087d1f | 1 | /* |
| guiott | 0:d177c0087d1f | 2 | * |
| guiott | 0:d177c0087d1f | 3 | * NMEA library |
| guiott | 0:d177c0087d1f | 4 | * URL: http://nmea.sourceforge.net |
| guiott | 0:d177c0087d1f | 5 | * Author: Tim (xtimor@gmail.com) |
| guiott | 0:d177c0087d1f | 6 | * Licence: http://www.gnu.org/licenses/lgpl.html |
| guiott | 0:d177c0087d1f | 7 | * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $ |
| guiott | 0:d177c0087d1f | 8 | * |
| guiott | 0:d177c0087d1f | 9 | */ |
| guiott | 0:d177c0087d1f | 10 | |
| guiott | 0:d177c0087d1f | 11 | /*! \file tok.h */ |
| guiott | 0:d177c0087d1f | 12 | |
| guiott | 0:d177c0087d1f | 13 | #include "nmea/tok.h" |
| guiott | 0:d177c0087d1f | 14 | |
| guiott | 0:d177c0087d1f | 15 | #include <stdarg.h> |
| guiott | 0:d177c0087d1f | 16 | #include <stdlib.h> |
| guiott | 0:d177c0087d1f | 17 | #include <stdio.h> |
| guiott | 0:d177c0087d1f | 18 | #include <ctype.h> |
| guiott | 0:d177c0087d1f | 19 | #include <string.h> |
| guiott | 0:d177c0087d1f | 20 | #include <limits.h> |
| guiott | 0:d177c0087d1f | 21 | |
| guiott | 0:d177c0087d1f | 22 | #define NMEA_TOKS_COMPARE (1) |
| guiott | 0:d177c0087d1f | 23 | #define NMEA_TOKS_PERCENT (2) |
| guiott | 0:d177c0087d1f | 24 | #define NMEA_TOKS_WIDTH (3) |
| guiott | 0:d177c0087d1f | 25 | #define NMEA_TOKS_TYPE (4) |
| guiott | 0:d177c0087d1f | 26 | |
| guiott | 0:d177c0087d1f | 27 | /** |
| guiott | 0:d177c0087d1f | 28 | * \brief Calculate control sum of binary buffer |
| guiott | 0:d177c0087d1f | 29 | */ |
| guiott | 0:d177c0087d1f | 30 | int nmea_calc_crc(const char *buff, int buff_sz) |
| guiott | 0:d177c0087d1f | 31 | { |
| guiott | 0:d177c0087d1f | 32 | int chsum = 0, |
| guiott | 0:d177c0087d1f | 33 | it; |
| guiott | 0:d177c0087d1f | 34 | |
| guiott | 0:d177c0087d1f | 35 | for(it = 0; it < buff_sz; ++it) |
| guiott | 0:d177c0087d1f | 36 | chsum ^= (int)buff[it]; |
| guiott | 0:d177c0087d1f | 37 | |
| guiott | 0:d177c0087d1f | 38 | return chsum; |
| guiott | 0:d177c0087d1f | 39 | } |
| guiott | 0:d177c0087d1f | 40 | |
| guiott | 0:d177c0087d1f | 41 | /** |
| guiott | 0:d177c0087d1f | 42 | * \brief Convert string to number |
| guiott | 0:d177c0087d1f | 43 | */ |
| guiott | 0:d177c0087d1f | 44 | int nmea_atoi(const char *str, int str_sz, int radix) |
| guiott | 0:d177c0087d1f | 45 | { |
| guiott | 0:d177c0087d1f | 46 | char *tmp_ptr; |
| guiott | 0:d177c0087d1f | 47 | char buff[NMEA_CONVSTR_BUF]; |
| guiott | 0:d177c0087d1f | 48 | int res = 0; |
| guiott | 0:d177c0087d1f | 49 | |
| guiott | 0:d177c0087d1f | 50 | if(str_sz < NMEA_CONVSTR_BUF) |
| guiott | 0:d177c0087d1f | 51 | { |
| guiott | 0:d177c0087d1f | 52 | memcpy(&buff[0], str, str_sz); |
| guiott | 0:d177c0087d1f | 53 | buff[str_sz] = '\0'; |
| guiott | 0:d177c0087d1f | 54 | res = strtol(&buff[0], &tmp_ptr, radix); |
| guiott | 0:d177c0087d1f | 55 | } |
| guiott | 0:d177c0087d1f | 56 | |
| guiott | 0:d177c0087d1f | 57 | return res; |
| guiott | 0:d177c0087d1f | 58 | } |
| guiott | 0:d177c0087d1f | 59 | |
| guiott | 0:d177c0087d1f | 60 | /** |
| guiott | 0:d177c0087d1f | 61 | * \brief Convert string to fraction number |
| guiott | 0:d177c0087d1f | 62 | */ |
| guiott | 0:d177c0087d1f | 63 | double nmea_atof(const char *str, int str_sz) |
| guiott | 0:d177c0087d1f | 64 | { |
| guiott | 0:d177c0087d1f | 65 | char *tmp_ptr; |
| guiott | 0:d177c0087d1f | 66 | char buff[NMEA_CONVSTR_BUF]; |
| guiott | 0:d177c0087d1f | 67 | double res = 0; |
| guiott | 0:d177c0087d1f | 68 | |
| guiott | 0:d177c0087d1f | 69 | if(str_sz < NMEA_CONVSTR_BUF) |
| guiott | 0:d177c0087d1f | 70 | { |
| guiott | 0:d177c0087d1f | 71 | memcpy(&buff[0], str, str_sz); |
| guiott | 0:d177c0087d1f | 72 | buff[str_sz] = '\0'; |
| guiott | 0:d177c0087d1f | 73 | res = strtod(&buff[0], &tmp_ptr); |
| guiott | 0:d177c0087d1f | 74 | } |
| guiott | 0:d177c0087d1f | 75 | |
| guiott | 0:d177c0087d1f | 76 | return res; |
| guiott | 0:d177c0087d1f | 77 | } |
| guiott | 0:d177c0087d1f | 78 | |
| guiott | 0:d177c0087d1f | 79 | /** |
| guiott | 0:d177c0087d1f | 80 | * \brief Formating string (like standart printf) with CRC tail (*CRC) |
| guiott | 0:d177c0087d1f | 81 | */ |
| guiott | 0:d177c0087d1f | 82 | int nmea_printf(char *buff, int buff_sz, const char *format, ...) |
| guiott | 0:d177c0087d1f | 83 | { |
| guiott | 0:d177c0087d1f | 84 | int retval, add = 0; |
| guiott | 0:d177c0087d1f | 85 | va_list arg_ptr; |
| guiott | 0:d177c0087d1f | 86 | |
| guiott | 0:d177c0087d1f | 87 | if(buff_sz <= 0) |
| guiott | 0:d177c0087d1f | 88 | return 0; |
| guiott | 0:d177c0087d1f | 89 | |
| guiott | 0:d177c0087d1f | 90 | va_start(arg_ptr, format); |
| guiott | 0:d177c0087d1f | 91 | |
| guiott | 0:d177c0087d1f | 92 | retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); |
| guiott | 0:d177c0087d1f | 93 | |
| guiott | 0:d177c0087d1f | 94 | if(retval > 0) |
| guiott | 0:d177c0087d1f | 95 | { |
| guiott | 0:d177c0087d1f | 96 | add = NMEA_POSIX(snprintf)( |
| guiott | 0:d177c0087d1f | 97 | buff + retval, buff_sz - retval, "*%02x\r\n", |
| guiott | 0:d177c0087d1f | 98 | nmea_calc_crc(buff + 1, retval - 1)); |
| guiott | 0:d177c0087d1f | 99 | } |
| guiott | 0:d177c0087d1f | 100 | |
| guiott | 0:d177c0087d1f | 101 | retval += add; |
| guiott | 0:d177c0087d1f | 102 | |
| guiott | 0:d177c0087d1f | 103 | if(retval < 0 || retval > buff_sz) |
| guiott | 0:d177c0087d1f | 104 | { |
| guiott | 0:d177c0087d1f | 105 | memset(buff, ' ', buff_sz); |
| guiott | 0:d177c0087d1f | 106 | retval = buff_sz; |
| guiott | 0:d177c0087d1f | 107 | } |
| guiott | 0:d177c0087d1f | 108 | |
| guiott | 0:d177c0087d1f | 109 | va_end(arg_ptr); |
| guiott | 0:d177c0087d1f | 110 | |
| guiott | 0:d177c0087d1f | 111 | return retval; |
| guiott | 0:d177c0087d1f | 112 | } |
| guiott | 0:d177c0087d1f | 113 | |
| guiott | 0:d177c0087d1f | 114 | /** |
| guiott | 0:d177c0087d1f | 115 | * \brief Analyse string (specificate for NMEA sentences) |
| guiott | 0:d177c0087d1f | 116 | */ |
| guiott | 0:d177c0087d1f | 117 | int nmea_scanf(const char *buff, int buff_sz, const char *format, ...) |
| guiott | 0:d177c0087d1f | 118 | { |
| guiott | 0:d177c0087d1f | 119 | const char *beg_tok; |
| guiott | 0:d177c0087d1f | 120 | const char *end_buf = buff + buff_sz; |
| guiott | 0:d177c0087d1f | 121 | |
| guiott | 0:d177c0087d1f | 122 | va_list arg_ptr; |
| guiott | 0:d177c0087d1f | 123 | int tok_type = NMEA_TOKS_COMPARE; |
| guiott | 0:d177c0087d1f | 124 | int width = 0; |
| guiott | 0:d177c0087d1f | 125 | const char *beg_fmt = 0; |
| guiott | 0:d177c0087d1f | 126 | int snum = 0, unum = 0; |
| guiott | 0:d177c0087d1f | 127 | |
| guiott | 0:d177c0087d1f | 128 | int tok_count = 0; |
| guiott | 0:d177c0087d1f | 129 | void *parg_target; |
| guiott | 0:d177c0087d1f | 130 | |
| guiott | 0:d177c0087d1f | 131 | va_start(arg_ptr, format); |
| guiott | 0:d177c0087d1f | 132 | |
| guiott | 0:d177c0087d1f | 133 | for(; *format && buff < end_buf; ++format) |
| guiott | 0:d177c0087d1f | 134 | { |
| guiott | 0:d177c0087d1f | 135 | switch(tok_type) |
| guiott | 0:d177c0087d1f | 136 | { |
| guiott | 0:d177c0087d1f | 137 | case NMEA_TOKS_COMPARE: |
| guiott | 0:d177c0087d1f | 138 | if('%' == *format) |
| guiott | 0:d177c0087d1f | 139 | tok_type = NMEA_TOKS_PERCENT; |
| guiott | 0:d177c0087d1f | 140 | else if(*buff++ != *format) |
| guiott | 0:d177c0087d1f | 141 | goto fail; |
| guiott | 0:d177c0087d1f | 142 | break; |
| guiott | 0:d177c0087d1f | 143 | case NMEA_TOKS_PERCENT: |
| guiott | 0:d177c0087d1f | 144 | width = 0; |
| guiott | 0:d177c0087d1f | 145 | beg_fmt = format; |
| guiott | 0:d177c0087d1f | 146 | tok_type = NMEA_TOKS_WIDTH; |
| guiott | 0:d177c0087d1f | 147 | case NMEA_TOKS_WIDTH: |
| guiott | 0:d177c0087d1f | 148 | if(isdigit(*format)) |
| guiott | 0:d177c0087d1f | 149 | break; |
| guiott | 0:d177c0087d1f | 150 | { |
| guiott | 0:d177c0087d1f | 151 | tok_type = NMEA_TOKS_TYPE; |
| guiott | 0:d177c0087d1f | 152 | if(format > beg_fmt) |
| guiott | 0:d177c0087d1f | 153 | width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10); |
| guiott | 0:d177c0087d1f | 154 | } |
| guiott | 0:d177c0087d1f | 155 | case NMEA_TOKS_TYPE: |
| guiott | 0:d177c0087d1f | 156 | beg_tok = buff; |
| guiott | 0:d177c0087d1f | 157 | |
| guiott | 0:d177c0087d1f | 158 | if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) |
| guiott | 0:d177c0087d1f | 159 | width = 1; |
| guiott | 0:d177c0087d1f | 160 | |
| guiott | 0:d177c0087d1f | 161 | if(width) |
| guiott | 0:d177c0087d1f | 162 | { |
| guiott | 0:d177c0087d1f | 163 | if(buff + width <= end_buf) |
| guiott | 0:d177c0087d1f | 164 | buff += width; |
| guiott | 0:d177c0087d1f | 165 | else |
| guiott | 0:d177c0087d1f | 166 | goto fail; |
| guiott | 0:d177c0087d1f | 167 | } |
| guiott | 0:d177c0087d1f | 168 | else |
| guiott | 0:d177c0087d1f | 169 | { |
| guiott | 0:d177c0087d1f | 170 | if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) |
| guiott | 0:d177c0087d1f | 171 | buff = end_buf; |
| guiott | 0:d177c0087d1f | 172 | } |
| guiott | 0:d177c0087d1f | 173 | |
| guiott | 0:d177c0087d1f | 174 | if(buff > end_buf) |
| guiott | 0:d177c0087d1f | 175 | goto fail; |
| guiott | 0:d177c0087d1f | 176 | |
| guiott | 0:d177c0087d1f | 177 | tok_type = NMEA_TOKS_COMPARE; |
| guiott | 0:d177c0087d1f | 178 | tok_count++; |
| guiott | 0:d177c0087d1f | 179 | |
| guiott | 0:d177c0087d1f | 180 | parg_target = 0; width = (int)(buff - beg_tok); |
| guiott | 0:d177c0087d1f | 181 | |
| guiott | 0:d177c0087d1f | 182 | switch(*format) |
| guiott | 0:d177c0087d1f | 183 | { |
| guiott | 0:d177c0087d1f | 184 | case 'c': |
| guiott | 0:d177c0087d1f | 185 | case 'C': |
| guiott | 0:d177c0087d1f | 186 | parg_target = (void *)va_arg(arg_ptr, char *); |
| guiott | 0:d177c0087d1f | 187 | if(width && 0 != (parg_target)) |
| guiott | 0:d177c0087d1f | 188 | *((char *)parg_target) = *beg_tok; |
| guiott | 0:d177c0087d1f | 189 | break; |
| guiott | 0:d177c0087d1f | 190 | case 's': |
| guiott | 0:d177c0087d1f | 191 | case 'S': |
| guiott | 0:d177c0087d1f | 192 | parg_target = (void *)va_arg(arg_ptr, char *); |
| guiott | 0:d177c0087d1f | 193 | if(width && 0 != (parg_target)) |
| guiott | 0:d177c0087d1f | 194 | { |
| guiott | 0:d177c0087d1f | 195 | memcpy(parg_target, beg_tok, width); |
| guiott | 0:d177c0087d1f | 196 | ((char *)parg_target)[width] = '\0'; |
| guiott | 0:d177c0087d1f | 197 | } |
| guiott | 0:d177c0087d1f | 198 | break; |
| guiott | 0:d177c0087d1f | 199 | case 'f': |
| guiott | 0:d177c0087d1f | 200 | case 'g': |
| guiott | 0:d177c0087d1f | 201 | case 'G': |
| guiott | 0:d177c0087d1f | 202 | case 'e': |
| guiott | 0:d177c0087d1f | 203 | case 'E': |
| guiott | 0:d177c0087d1f | 204 | parg_target = (void *)va_arg(arg_ptr, double *); |
| guiott | 0:d177c0087d1f | 205 | if(width && 0 != (parg_target)) |
| guiott | 0:d177c0087d1f | 206 | *((double *)parg_target) = nmea_atof(beg_tok, width); |
| guiott | 0:d177c0087d1f | 207 | break; |
| guiott | 0:d177c0087d1f | 208 | }; |
| guiott | 0:d177c0087d1f | 209 | |
| guiott | 0:d177c0087d1f | 210 | if(parg_target) |
| guiott | 0:d177c0087d1f | 211 | break; |
| guiott | 0:d177c0087d1f | 212 | if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) |
| guiott | 0:d177c0087d1f | 213 | break; |
| guiott | 0:d177c0087d1f | 214 | if(!width) |
| guiott | 0:d177c0087d1f | 215 | break; |
| guiott | 0:d177c0087d1f | 216 | |
| guiott | 0:d177c0087d1f | 217 | switch(*format) |
| guiott | 0:d177c0087d1f | 218 | { |
| guiott | 0:d177c0087d1f | 219 | case 'd': |
| guiott | 0:d177c0087d1f | 220 | case 'i': |
| guiott | 0:d177c0087d1f | 221 | snum = nmea_atoi(beg_tok, width, 10); |
| guiott | 0:d177c0087d1f | 222 | memcpy(parg_target, &snum, sizeof(int)); |
| guiott | 0:d177c0087d1f | 223 | break; |
| guiott | 0:d177c0087d1f | 224 | case 'u': |
| guiott | 0:d177c0087d1f | 225 | unum = nmea_atoi(beg_tok, width, 10); |
| guiott | 0:d177c0087d1f | 226 | memcpy(parg_target, &unum, sizeof(unsigned int)); |
| guiott | 0:d177c0087d1f | 227 | break; |
| guiott | 0:d177c0087d1f | 228 | case 'x': |
| guiott | 0:d177c0087d1f | 229 | case 'X': |
| guiott | 0:d177c0087d1f | 230 | unum = nmea_atoi(beg_tok, width, 16); |
| guiott | 0:d177c0087d1f | 231 | memcpy(parg_target, &unum, sizeof(unsigned int)); |
| guiott | 0:d177c0087d1f | 232 | break; |
| guiott | 0:d177c0087d1f | 233 | case 'o': |
| guiott | 0:d177c0087d1f | 234 | unum = nmea_atoi(beg_tok, width, 8); |
| guiott | 0:d177c0087d1f | 235 | memcpy(parg_target, &unum, sizeof(unsigned int)); |
| guiott | 0:d177c0087d1f | 236 | break; |
| guiott | 0:d177c0087d1f | 237 | default: |
| guiott | 0:d177c0087d1f | 238 | goto fail; |
| guiott | 0:d177c0087d1f | 239 | }; |
| guiott | 0:d177c0087d1f | 240 | |
| guiott | 0:d177c0087d1f | 241 | break; |
| guiott | 0:d177c0087d1f | 242 | }; |
| guiott | 0:d177c0087d1f | 243 | } |
| guiott | 0:d177c0087d1f | 244 | |
| guiott | 0:d177c0087d1f | 245 | fail: |
| guiott | 0:d177c0087d1f | 246 | |
| guiott | 0:d177c0087d1f | 247 | va_end(arg_ptr); |
| guiott | 0:d177c0087d1f | 248 | |
| guiott | 0:d177c0087d1f | 249 | return tok_count; |
| guiott | 0:d177c0087d1f | 250 | } |