Toshihisa T / Mbed 2 deprecated S1315FLogger

Skytraq S1315F-RAW-EVK Logger

Dependencies:   TextLCD mbed

libT/portable/NMEA_parse.c@2:7eb11afe02bd, 2010-12-19 (annotated)

Committer:
tosihisa
Date:
Sun Dec 19 09:11:48 2010 +0000
Revision:
2:7eb11afe02bd
Parent:
0:e0ec137da369 
Add \"SYSTEM RESTART\" at boot time.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tosihisa 0:e0ec137da369 1
tosihisa 0:e0ec137da369 2 #include "NMEA_parse.h"
tosihisa 0:e0ec137da369 3
tosihisa 0:e0ec137da369 4 #ifndef NULL
tosihisa 0:e0ec137da369 5 # define NULL ((void *)0)
tosihisa 0:e0ec137da369 6 #endif
tosihisa 0:e0ec137da369 7
tosihisa 0:e0ec137da369 8 /*
tosihisa 0:e0ec137da369 9 nmealib GM-316 FV-M11
tosihisa 0:e0ec137da369 10 GGA Global Positioning System Fix Data. O O O
tosihisa 0:e0ec137da369 11 GSA GNSS DOP and Active Satellites O O O
tosihisa 0:e0ec137da369 12 GSV GNSS Satellites in View O O O
tosihisa 0:e0ec137da369 13 RMC Recommended Minimum Navigation Information O O O
tosihisa 0:e0ec137da369 14 VTG Course Over Ground and Ground Speed O O O
tosihisa 0:e0ec137da369 15 GGL Geographic Position – Latitude / Longitude X O O
tosihisa 0:e0ec137da369 16 ZDA Time & Date X O(synchronized to PPS) O
tosihisa 0:e0ec137da369 17 */
tosihisa 0:e0ec137da369 18
tosihisa 0:e0ec137da369 19 static unsigned char _libNMEA_atoi(unsigned char c)
tosihisa 0:e0ec137da369 20 {
tosihisa 0:e0ec137da369 21 if((c >= (unsigned char)('0')) && (c <= (unsigned char)('9')))
tosihisa 0:e0ec137da369 22 return (c - (unsigned char)('0'));
tosihisa 0:e0ec137da369 23 if((c >= (unsigned char)('A')) && (c <= (unsigned char)('F')))
tosihisa 0:e0ec137da369 24 return (c - (unsigned char)('A')) + 10;
tosihisa 0:e0ec137da369 25 return (c - (unsigned char)('a')) + 10;
tosihisa 0:e0ec137da369 26 }
tosihisa 0:e0ec137da369 27 short libNMEA_Parse1Char(unsigned char c,libNMEA_data_t *buffer)
tosihisa 0:e0ec137da369 28 {
tosihisa 0:e0ec137da369 29 register short cnewst = 0;
tosihisa 0:e0ec137da369 30
tosihisa 0:e0ec137da369 31 /* It does again at the time of beginning after one line is obtained it. */
tosihisa 0:e0ec137da369 32 /* It does again after the syntax error at the time of beginning. */
tosihisa 0:e0ec137da369 33 /* １行を得た後ならば，再度初めから */
tosihisa 0:e0ec137da369 34 /* パースエラーの後も，再度初めから */
tosihisa 0:e0ec137da369 35 if((buffer->cjobst >= LIBNMEA_PARSE_COMPLETE) || (buffer->cjobst < LIBNMEA_PARSE_NONE)){
tosihisa 0:e0ec137da369 36 buffer->cjobst = LIBNMEA_PARSE_NONE;
tosihisa 0:e0ec137da369 37 }
tosihisa 0:e0ec137da369 38
tosihisa 0:e0ec137da369 39 if(buffer->cjobst == LIBNMEA_PARSE_NONE){ /* Wait '$' */
tosihisa 0:e0ec137da369 40 if((c != (unsigned char)('$')) && (c != (unsigned char)('!'))){
tosihisa 0:e0ec137da369 41 return buffer->cjobst;
tosihisa 0:e0ec137da369 42 }
tosihisa 0:e0ec137da369 43 buffer->rawLength = 0;
tosihisa 0:e0ec137da369 44 buffer->sum = 0;
tosihisa 0:e0ec137da369 45 }
tosihisa 0:e0ec137da369 46
tosihisa 0:e0ec137da369 47 /* The size check is previously done because it stores it here in the buffer now. */
tosihisa 0:e0ec137da369 48 if(buffer->rawLength >= (sizeof(buffer->raw)-2)){
tosihisa 0:e0ec137da369 49 buffer->cjobst = -3; /* Buffer size overflow */
tosihisa 0:e0ec137da369 50 return buffer->cjobst;
tosihisa 0:e0ec137da369 51 }
tosihisa 0:e0ec137da369 52
tosihisa 0:e0ec137da369 53 buffer->raw[buffer->rawLength] = c;
tosihisa 0:e0ec137da369 54 buffer->rawLength += 1;
tosihisa 0:e0ec137da369 55 buffer->raw[buffer->rawLength] = (unsigned char)('\0');
tosihisa 0:e0ec137da369 56
tosihisa 0:e0ec137da369 57 cnewst = -1; /* Syntax error (It initializes it by the default value.) */
tosihisa 0:e0ec137da369 58
tosihisa 0:e0ec137da369 59 switch(buffer->cjobst){
tosihisa 0:e0ec137da369 60 case LIBNMEA_PARSE_NONE:
tosihisa 0:e0ec137da369 61 cnewst = 1;
tosihisa 0:e0ec137da369 62 break;
tosihisa 0:e0ec137da369 63 case 1: /* データ待ち */
tosihisa 0:e0ec137da369 64 cnewst = buffer->cjobst;
tosihisa 0:e0ec137da369 65 if(c == (unsigned char)('*')){
tosihisa 0:e0ec137da369 66 cnewst += 1; /* チェックサム１文字目を得る */
tosihisa 0:e0ec137da369 67 } else {
tosihisa 0:e0ec137da369 68 buffer->sum = buffer->sum ^ c; /* チェックサム計算 */
tosihisa 0:e0ec137da369 69 }
tosihisa 0:e0ec137da369 70 break;
tosihisa 0:e0ec137da369 71 case 2: /* チェックサム１文字目 */
tosihisa 0:e0ec137da369 72 buffer->sumwk = _libNMEA_atoi(c);
tosihisa 0:e0ec137da369 73 buffer->sumwk = buffer->sumwk << 4;
tosihisa 0:e0ec137da369 74 cnewst = buffer->cjobst + 1; /* チェックサム２文字目を得る */
tosihisa 0:e0ec137da369 75 break;
tosihisa 0:e0ec137da369 76 case 3: /* チェックサム２文字目 */
tosihisa 0:e0ec137da369 77 if((buffer->sumwk |= _libNMEA_atoi(c)) == buffer->sum){
tosihisa 0:e0ec137da369 78 cnewst = LIBNMEA_PARSE_SUMOK; /* CR待ち */
tosihisa 0:e0ec137da369 79 } else {
tosihisa 0:e0ec137da369 80 //printf("SUM:%02x/%02x\n",buffer->sumwk,buffer->sum);
tosihisa 0:e0ec137da369 81 cnewst = -2; /* チェックサムエラー */
tosihisa 0:e0ec137da369 82 }
tosihisa 0:e0ec137da369 83 break;
tosihisa 0:e0ec137da369 84 case LIBNMEA_PARSE_SUMOK: /* CR 待ち */
tosihisa 0:e0ec137da369 85 if(c == (unsigned char)0x0d){
tosihisa 0:e0ec137da369 86 cnewst = buffer->cjobst + 1; /* LF待ち */
tosihisa 0:e0ec137da369 87 }
tosihisa 0:e0ec137da369 88 break;
tosihisa 0:e0ec137da369 89 case LIBNMEA_PARSE_SUMOK+1: /* LF 待ち */
tosihisa 0:e0ec137da369 90 if(c == (unsigned char)0x0a){
tosihisa 0:e0ec137da369 91 cnewst = LIBNMEA_PARSE_COMPLETE; /* センテンス完了 */
tosihisa 0:e0ec137da369 92 }
tosihisa 0:e0ec137da369 93 break;
tosihisa 0:e0ec137da369 94 }
tosihisa 0:e0ec137da369 95
tosihisa 0:e0ec137da369 96 buffer->cjobst = cnewst;
tosihisa 0:e0ec137da369 97 return buffer->cjobst;
tosihisa 0:e0ec137da369 98 }
tosihisa 0:e0ec137da369 99
tosihisa 0:e0ec137da369 100 /* 10進数固定 */
tosihisa 0:e0ec137da369 101 short libNMEA_atol(unsigned char *numstr,long *val,long *length,unsigned char **nextptr)
tosihisa 0:e0ec137da369 102 {
tosihisa 0:e0ec137da369 103 long retval = 0;
tosihisa 0:e0ec137da369 104 int i = 0;
tosihisa 0:e0ec137da369 105 int mi = 0;
tosihisa 0:e0ec137da369 106 unsigned char c;
tosihisa 0:e0ec137da369 107
tosihisa 0:e0ec137da369 108 *length = 1;
tosihisa 0:e0ec137da369 109
tosihisa 0:e0ec137da369 110 if(*(numstr + i) == (unsigned char)('-')){
tosihisa 0:e0ec137da369 111 mi = 1;
tosihisa 0:e0ec137da369 112 i++;
tosihisa 0:e0ec137da369 113 }
tosihisa 0:e0ec137da369 114
tosihisa 0:e0ec137da369 115 for(;*(numstr + i) != (unsigned char)('\0');i++){
tosihisa 0:e0ec137da369 116 c = *(numstr + i);
tosihisa 0:e0ec137da369 117 if((c >= (unsigned char)('0')) && (c <= (unsigned char)('9'))){
tosihisa 0:e0ec137da369 118 c = c - (unsigned char)('0');
tosihisa 0:e0ec137da369 119 retval = (retval * 10) + c;
tosihisa 0:e0ec137da369 120 *length = *length * 10;
tosihisa 0:e0ec137da369 121 } else {
tosihisa 0:e0ec137da369 122 break;
tosihisa 0:e0ec137da369 123 }
tosihisa 0:e0ec137da369 124 }
tosihisa 0:e0ec137da369 125 if((nextptr != NULL) && (*nextptr != NULL)){
tosihisa 0:e0ec137da369 126 *nextptr = numstr + i;
tosihisa 0:e0ec137da369 127 }
tosihisa 0:e0ec137da369 128 *val = (mi) ? (0-retval) : retval;
tosihisa 0:e0ec137da369 129 return 0;
tosihisa 0:e0ec137da369 130 }
tosihisa 0:e0ec137da369 131
tosihisa 0:e0ec137da369 132 short libNMEA_atod(unsigned char *numstr,libNMEA_realnum_t *val,unsigned char **nextptr)
tosihisa 0:e0ec137da369 133 {
tosihisa 0:e0ec137da369 134 long lval = 0;
tosihisa 0:e0ec137da369 135 libNMEA_realnum_t e = 0.0;
tosihisa 0:e0ec137da369 136 libNMEA_realnum_t f = 0.0;
tosihisa 0:e0ec137da369 137 unsigned char *nptr;
tosihisa 0:e0ec137da369 138 long length;
tosihisa 0:e0ec137da369 139
tosihisa 0:e0ec137da369 140 /* TODO:符号チェックを入れる事 */
tosihisa 0:e0ec137da369 141
tosihisa 0:e0ec137da369 142 /* 整数部 */
tosihisa 0:e0ec137da369 143 libNMEA_atol(numstr,&lval,&length,&nptr);
tosihisa 0:e0ec137da369 144 e = (libNMEA_realnum_t)lval;
tosihisa 0:e0ec137da369 145
tosihisa 0:e0ec137da369 146 /* 小数部 */
tosihisa 0:e0ec137da369 147 if(*nptr == (unsigned char)('.')){
tosihisa 0:e0ec137da369 148 nptr++;
tosihisa 0:e0ec137da369 149 libNMEA_atol(nptr,&lval,&length,&nptr);
tosihisa 0:e0ec137da369 150 f = ((libNMEA_realnum_t)lval) / ((libNMEA_realnum_t)length);
tosihisa 0:e0ec137da369 151 }
tosihisa 0:e0ec137da369 152
tosihisa 0:e0ec137da369 153 if((nextptr != NULL) && (*nextptr != NULL)){
tosihisa 0:e0ec137da369 154 *nextptr = nptr;
tosihisa 0:e0ec137da369 155 }
tosihisa 0:e0ec137da369 156 *val = e + f;
tosihisa 0:e0ec137da369 157 return 0;
tosihisa 0:e0ec137da369 158 }
tosihisa 0:e0ec137da369 159
tosihisa 0:e0ec137da369 160 /* GPS の緯度経度 ddmm.mmmm(dddmm.mmmm) 形式の文字列を，
tosihisa 0:e0ec137da369 161 dd.dddd の libNMEA_realnum_t 形式に変換する． */
tosihisa 0:e0ec137da369 162 /* http://www.circuitcellar.com/library/print/1000/Stefan123/4.htm */
tosihisa 0:e0ec137da369 163 short libNMEA_atodeg(unsigned char *numstr,libNMEA_realnum_t *val)
tosihisa 0:e0ec137da369 164 {
tosihisa 0:e0ec137da369 165 volatile long wk = 0;
tosihisa 0:e0ec137da369 166 volatile long deg_l = 0;
tosihisa 0:e0ec137da369 167 long length = 0;
tosihisa 0:e0ec137da369 168 libNMEA_realnum_t mm_d = 0;
tosihisa 0:e0ec137da369 169 unsigned char *nextptr;
tosihisa 0:e0ec137da369 170
tosihisa 0:e0ec137da369 171 /* ddmm 部を取り出す */
tosihisa 0:e0ec137da369 172 libNMEA_atol(numstr,(long *)&wk,&length,&nextptr);
tosihisa 0:e0ec137da369 173
tosihisa 0:e0ec137da369 174 /* dd を得る */
tosihisa 0:e0ec137da369 175 deg_l = wk / 100;
tosihisa 0:e0ec137da369 176
tosihisa 0:e0ec137da369 177 /* .mmmm 部を取り出す */
tosihisa 0:e0ec137da369 178 libNMEA_atod(nextptr,&mm_d,&nextptr);
tosihisa 0:e0ec137da369 179
tosihisa 0:e0ec137da369 180 /* mm.mmmm にする */
tosihisa 0:e0ec137da369 181 mm_d = ((libNMEA_realnum_t)(wk % 100)) + mm_d;
tosihisa 0:e0ec137da369 182 /* 0.dddd に変換する */
tosihisa 0:e0ec137da369 183 mm_d = (mm_d / 60.0);
tosihisa 0:e0ec137da369 184 /* もし，mm_d が 1.0 よりも大きい場合，mm.mmmm は60分を超える値であり，
tosihisa 0:e0ec137da369 185 GPS の緯度経度値として適切ではない */
tosihisa 0:e0ec137da369 186 if(mm_d > 1.0){
tosihisa 0:e0ec137da369 187 return -1;
tosihisa 0:e0ec137da369 188 }
tosihisa 0:e0ec137da369 189
tosihisa 0:e0ec137da369 190 *val = ((libNMEA_realnum_t)deg_l) + mm_d; /* dd.dddd 形式に変換 */
tosihisa 0:e0ec137da369 191
tosihisa 0:e0ec137da369 192 return 0;
tosihisa 0:e0ec137da369 193 }
tosihisa 0:e0ec137da369 194
tosihisa 0:e0ec137da369 195 #if 0
tosihisa 0:e0ec137da369 196 static short _libNMEA_print_args(libNMEA_data_t *buffer)
tosihisa 0:e0ec137da369 197 {
tosihisa 0:e0ec137da369 198 int i;
tosihisa 0:e0ec137da369 199 extern int printf(const char *format, ...);
tosihisa 0:e0ec137da369 200
tosihisa 0:e0ec137da369 201 printf("ARGC %5d:",buffer->argc);
tosihisa 0:e0ec137da369 202 for(i=0;i < buffer->argc;i++){
tosihisa 0:e0ec137da369 203 printf("[%s]",buffer->raw + buffer->argv[i]);
tosihisa 0:e0ec137da369 204 }
tosihisa 0:e0ec137da369 205 printf("\n");
tosihisa 0:e0ec137da369 206 return 0;
tosihisa 0:e0ec137da369 207 }
tosihisa 0:e0ec137da369 208 #endif
tosihisa 0:e0ec137da369 209
tosihisa 0:e0ec137da369 210 static void _libNMEA_String2Time(unsigned char *numstr,libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 211 {
tosihisa 0:e0ec137da369 212 long wk;
tosihisa 0:e0ec137da369 213 long length;
tosihisa 0:e0ec137da369 214 unsigned char *nextptr;
tosihisa 0:e0ec137da369 215
tosihisa 0:e0ec137da369 216 libNMEA_atol(numstr,&wk,&length,&nextptr);
tosihisa 0:e0ec137da369 217 GPSinfo->time.sec = wk % 100;
tosihisa 0:e0ec137da369 218 wk = wk/100;
tosihisa 0:e0ec137da369 219 GPSinfo->time.min = wk % 100;
tosihisa 0:e0ec137da369 220 wk = wk/100;
tosihisa 0:e0ec137da369 221 GPSinfo->time.hour = wk % 100;
tosihisa 0:e0ec137da369 222 GPSinfo->time.msec = 0;
tosihisa 0:e0ec137da369 223 if((nextptr != NULL) && (*nextptr == (unsigned char)('.'))){
tosihisa 0:e0ec137da369 224 libNMEA_atol(nextptr+1,&wk,&length,(unsigned char **)NULL);
tosihisa 0:e0ec137da369 225 GPSinfo->time.msec = wk;
tosihisa 0:e0ec137da369 226 }
tosihisa 0:e0ec137da369 227 }
tosihisa 0:e0ec137da369 228
tosihisa 0:e0ec137da369 229 static void _libNMEA_setInvalid(libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 230 {
tosihisa 0:e0ec137da369 231 GPSinfo->valid.status = 0;
tosihisa 0:e0ec137da369 232
tosihisa 0:e0ec137da369 233 GPSinfo->valid.positioning = 0;
tosihisa 0:e0ec137da369 234 GPSinfo->valid.active = 0;
tosihisa 0:e0ec137da369 235 }
tosihisa 0:e0ec137da369 236
tosihisa 0:e0ec137da369 237 /* Recommended Minimum Course Response Message */
tosihisa 0:e0ec137da369 238 static short _libNMEA_Parse1Line_GPRMC(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 239 {
tosihisa 0:e0ec137da369 240 unsigned char *nextptr;
tosihisa 0:e0ec137da369 241
tosihisa 0:e0ec137da369 242 if(buffer->argc != 13){
tosihisa 0:e0ec137da369 243 return -2; /* データの数がおかしい */
tosihisa 0:e0ec137da369 244 }
tosihisa 0:e0ec137da369 245 //_libNMEA_print_args(buffer);
tosihisa 0:e0ec137da369 246
tosihisa 0:e0ec137da369 247 /* まず，真っ先に見るのは，GPS データのステータス(妥当性) */
tosihisa 0:e0ec137da369 248 if(*(buffer->raw+buffer->argv[2]+0) != (unsigned char)('A')){
tosihisa 0:e0ec137da369 249 /* GPS データ妥当性無し */
tosihisa 0:e0ec137da369 250 GPSinfo->valid.status = 0;
tosihisa 0:e0ec137da369 251 /* 妥当性が無いので，これ以上解析する必要は無いので終わる */
tosihisa 0:e0ec137da369 252 return 0;
tosihisa 0:e0ec137da369 253 }
tosihisa 0:e0ec137da369 254
tosihisa 0:e0ec137da369 255 /* 測位状態を得る */
tosihisa 0:e0ec137da369 256 switch(*(buffer->raw+buffer->argv[12]+0)){
tosihisa 0:e0ec137da369 257 case (unsigned char)('A'): /* 単独測位 */
tosihisa 0:e0ec137da369 258 case (unsigned char)('D'): /* DGPS */
tosihisa 0:e0ec137da369 259 GPSinfo->status.positioning = *(buffer->raw+buffer->argv[12]+0);
tosihisa 0:e0ec137da369 260 GPSinfo->valid.positioning = 1;
tosihisa 0:e0ec137da369 261 break;
tosihisa 0:e0ec137da369 262 default:
tosihisa 0:e0ec137da369 263 GPSinfo->status.positioning = (unsigned char)('N');
tosihisa 0:e0ec137da369 264 _libNMEA_setInvalid(GPSinfo);
tosihisa 0:e0ec137da369 265 return 0; /* 無効な測位状態なので返る */
tosihisa 0:e0ec137da369 266 }
tosihisa 0:e0ec137da369 267
tosihisa 0:e0ec137da369 268 /* *** 妥当性のあるデータ *** */
tosihisa 0:e0ec137da369 269
tosihisa 0:e0ec137da369 270 /* 緯度を得る */
tosihisa 0:e0ec137da369 271 libNMEA_atodeg( buffer->raw+buffer->argv[3]
tosihisa 0:e0ec137da369 272 ,&(GPSinfo->latlon.latitude) );
tosihisa 0:e0ec137da369 273 GPSinfo->latlon.lat_unit = *(buffer->raw+buffer->argv[4]);
tosihisa 0:e0ec137da369 274 GPSinfo->valid.latitude = 1;
tosihisa 0:e0ec137da369 275
tosihisa 0:e0ec137da369 276 /* 経度を得る */
tosihisa 0:e0ec137da369 277 libNMEA_atodeg( buffer->raw+buffer->argv[5]
tosihisa 0:e0ec137da369 278 ,&(GPSinfo->latlon.longitude ) );
tosihisa 0:e0ec137da369 279 GPSinfo->latlon.lon_unit = *(buffer->raw+buffer->argv[6]);
tosihisa 0:e0ec137da369 280 GPSinfo->valid.longitude = 1;
tosihisa 0:e0ec137da369 281
tosihisa 0:e0ec137da369 282 /* 対地速度（ノット）を得る */
tosihisa 0:e0ec137da369 283 libNMEA_atod( buffer->raw+buffer->argv[7],
tosihisa 0:e0ec137da369 284 &(GPSinfo->speed.ground),
tosihisa 0:e0ec137da369 285 &nextptr);
tosihisa 0:e0ec137da369 286 GPSinfo->valid.speed = 1;
tosihisa 0:e0ec137da369 287
tosihisa 0:e0ec137da369 288 /* 進行方向（度，真北）を得る */
tosihisa 0:e0ec137da369 289 libNMEA_atod( buffer->raw+buffer->argv[8],
tosihisa 0:e0ec137da369 290 &(GPSinfo->direction.trueNorth.deg),
tosihisa 0:e0ec137da369 291 &nextptr);
tosihisa 0:e0ec137da369 292 GPSinfo->valid.direction = 1;
tosihisa 0:e0ec137da369 293
tosihisa 0:e0ec137da369 294 GPSinfo->valid.timeRMC = 0;
tosihisa 0:e0ec137da369 295 if(1){
tosihisa 0:e0ec137da369 296 /* 現在時刻／日付を得る．*/
tosihisa 0:e0ec137da369 297 long wk;
tosihisa 0:e0ec137da369 298 long length;
tosihisa 0:e0ec137da369 299
tosihisa 0:e0ec137da369 300 _libNMEA_String2Time(buffer->raw+buffer->argv[1],GPSinfo);
tosihisa 0:e0ec137da369 301
tosihisa 0:e0ec137da369 302 libNMEA_atol(buffer->raw+buffer->argv[9],&wk,&length,&nextptr);
tosihisa 0:e0ec137da369 303 /* GPRMC は，西暦は下2桁しかないので，2000を加算する． */
tosihisa 0:e0ec137da369 304 GPSinfo->date.year = (wk % 100) + 2000; /* after year 2000. */
tosihisa 0:e0ec137da369 305 wk = wk/100;
tosihisa 0:e0ec137da369 306 GPSinfo->date.mon = wk % 100;
tosihisa 0:e0ec137da369 307 wk = wk/100;
tosihisa 0:e0ec137da369 308 GPSinfo->date.day = wk % 100;
tosihisa 0:e0ec137da369 309 GPSinfo->valid.timeRMC = 1;
tosihisa 0:e0ec137da369 310 }
tosihisa 0:e0ec137da369 311
tosihisa 0:e0ec137da369 312 /* ステータスは有効 */
tosihisa 0:e0ec137da369 313 GPSinfo->valid.status = 1;
tosihisa 0:e0ec137da369 314
tosihisa 0:e0ec137da369 315 return 0;
tosihisa 0:e0ec137da369 316 }
tosihisa 0:e0ec137da369 317
tosihisa 0:e0ec137da369 318 static short _libNMEA_Parse1Line_GPZDA(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 319 {
tosihisa 0:e0ec137da369 320 long wk;
tosihisa 0:e0ec137da369 321 long length;
tosihisa 0:e0ec137da369 322 unsigned char *nextptr;
tosihisa 0:e0ec137da369 323
tosihisa 0:e0ec137da369 324 if(buffer->argc != 7){
tosihisa 0:e0ec137da369 325 return -2;
tosihisa 0:e0ec137da369 326 }
tosihisa 0:e0ec137da369 327
tosihisa 0:e0ec137da369 328 GPSinfo->valid.timeZDA = 0;
tosihisa 0:e0ec137da369 329
tosihisa 0:e0ec137da369 330 _libNMEA_String2Time(buffer->raw+buffer->argv[1],GPSinfo);
tosihisa 0:e0ec137da369 331
tosihisa 0:e0ec137da369 332 libNMEA_atol(buffer->raw+buffer->argv[2],&wk,&length,&nextptr);
tosihisa 0:e0ec137da369 333 GPSinfo->date.day = wk;
tosihisa 0:e0ec137da369 334 libNMEA_atol(buffer->raw+buffer->argv[3],&wk,&length,&nextptr);
tosihisa 0:e0ec137da369 335 GPSinfo->date.mon = wk;
tosihisa 0:e0ec137da369 336 libNMEA_atol(buffer->raw+buffer->argv[4],&wk,&length,&nextptr);
tosihisa 0:e0ec137da369 337 GPSinfo->date.year = wk;
tosihisa 0:e0ec137da369 338
tosihisa 0:e0ec137da369 339 GPSinfo->valid.timeZDA = 1;
tosihisa 0:e0ec137da369 340
tosihisa 0:e0ec137da369 341 return 0;
tosihisa 0:e0ec137da369 342 }
tosihisa 0:e0ec137da369 343
tosihisa 0:e0ec137da369 344 /* GPGSA - GNSS DOP and Active Satellites */
tosihisa 0:e0ec137da369 345 /* 0:[$GPGSA],1:[A],2:[3],3:[05],4:[02],5:[26],6:[27],7:[09],8:[04],9:[15],10:[],11:[],12:[],13:[],14:[],15:[1.8],16:[1.0],17:[1.5]*11 */
tosihisa 0:e0ec137da369 346 static short _libNMEA_Parse1Line_GPGSA(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 347 {
tosihisa 0:e0ec137da369 348 if(buffer->argc != 18){
tosihisa 0:e0ec137da369 349 return -2;
tosihisa 0:e0ec137da369 350 }
tosihisa 0:e0ec137da369 351
tosihisa 0:e0ec137da369 352 switch( *(buffer->raw+buffer->argv[2]) ){
tosihisa 0:e0ec137da369 353 case '2': /* 2D */
tosihisa 0:e0ec137da369 354 case '3': /* 3D */
tosihisa 0:e0ec137da369 355 GPSinfo->status.active = *(buffer->raw+buffer->argv[2]);
tosihisa 0:e0ec137da369 356 GPSinfo->valid.active = 1;
tosihisa 0:e0ec137da369 357 break;
tosihisa 0:e0ec137da369 358 default: /* Fix not available */
tosihisa 0:e0ec137da369 359 _libNMEA_setInvalid(GPSinfo);
tosihisa 0:e0ec137da369 360 return 0; /* 無効な測位状態なので返る */
tosihisa 0:e0ec137da369 361 }
tosihisa 0:e0ec137da369 362
tosihisa 0:e0ec137da369 363 GPSinfo->valid.accuracy_pdop = 0;
tosihisa 0:e0ec137da369 364 libNMEA_atod( buffer->raw+buffer->argv[15],
tosihisa 0:e0ec137da369 365 &(GPSinfo->accuracy.p.dop),
tosihisa 0:e0ec137da369 366 (unsigned char **)NULL);
tosihisa 0:e0ec137da369 367 GPSinfo->valid.accuracy_pdop = 1;
tosihisa 0:e0ec137da369 368
tosihisa 0:e0ec137da369 369 GPSinfo->valid.accuracy_hdop = 0;
tosihisa 0:e0ec137da369 370 libNMEA_atod( buffer->raw+buffer->argv[16],
tosihisa 0:e0ec137da369 371 &(GPSinfo->accuracy.h.dop),
tosihisa 0:e0ec137da369 372 (unsigned char **)NULL);
tosihisa 0:e0ec137da369 373 GPSinfo->valid.accuracy_hdop = 1;
tosihisa 0:e0ec137da369 374
tosihisa 0:e0ec137da369 375 GPSinfo->valid.accuracy_vdop = 0;
tosihisa 0:e0ec137da369 376 libNMEA_atod( buffer->raw+buffer->argv[17],
tosihisa 0:e0ec137da369 377 &(GPSinfo->accuracy.v.dop),
tosihisa 0:e0ec137da369 378 (unsigned char **)NULL);
tosihisa 0:e0ec137da369 379 GPSinfo->valid.accuracy_vdop = 1;
tosihisa 0:e0ec137da369 380
tosihisa 0:e0ec137da369 381 return 0;
tosihisa 0:e0ec137da369 382 }
tosihisa 0:e0ec137da369 383
tosihisa 0:e0ec137da369 384 static short _libNMEA_Parse1Line_GP(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 385 {
tosihisa 0:e0ec137da369 386 unsigned char *sentenceID = (unsigned char *)0;
tosihisa 0:e0ec137da369 387
tosihisa 0:e0ec137da369 388 sentenceID = buffer->raw+buffer->argv[0];
tosihisa 0:e0ec137da369 389 sentenceID += 3; /* "$GP" をスキップ */
tosihisa 0:e0ec137da369 390
tosihisa 0:e0ec137da369 391 if( (*(sentenceID+0) == (unsigned char)('R'))
tosihisa 0:e0ec137da369 392 && (*(sentenceID+1) == (unsigned char)('M'))
tosihisa 0:e0ec137da369 393 && (*(sentenceID+2) == (unsigned char)('C'))){
tosihisa 0:e0ec137da369 394 return _libNMEA_Parse1Line_GPRMC(buffer,GPSinfo);
tosihisa 0:e0ec137da369 395 } else if( (*(sentenceID+0) == (unsigned char)('Z'))
tosihisa 0:e0ec137da369 396 && (*(sentenceID+1) == (unsigned char)('D'))
tosihisa 0:e0ec137da369 397 && (*(sentenceID+2) == (unsigned char)('A'))){
tosihisa 0:e0ec137da369 398 return _libNMEA_Parse1Line_GPZDA(buffer,GPSinfo);
tosihisa 0:e0ec137da369 399 } else if((*(sentenceID+0) == (unsigned char)('G'))
tosihisa 0:e0ec137da369 400 && (*(sentenceID+1) == (unsigned char)('S'))
tosihisa 0:e0ec137da369 401 && (*(sentenceID+2) == (unsigned char)('A'))){
tosihisa 0:e0ec137da369 402 return _libNMEA_Parse1Line_GPGSA(buffer,GPSinfo);
tosihisa 0:e0ec137da369 403 }
tosihisa 0:e0ec137da369 404
tosihisa 0:e0ec137da369 405 return -2; /* このセンテンスは処理しませんでした */
tosihisa 0:e0ec137da369 406 }
tosihisa 0:e0ec137da369 407
tosihisa 0:e0ec137da369 408 #ifdef DEBUG /* { */
tosihisa 0:e0ec137da369 409 static short max_argc = 0;
tosihisa 0:e0ec137da369 410 #endif /* } */
tosihisa 0:e0ec137da369 411
tosihisa 0:e0ec137da369 412 short libNMEA_Parse1Line(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
tosihisa 0:e0ec137da369 413 {
tosihisa 0:e0ec137da369 414 int i;
tosihisa 0:e0ec137da369 415 unsigned char *tokerID = (unsigned char *)0;
tosihisa 0:e0ec137da369 416
tosihisa 0:e0ec137da369 417 buffer->argc = 0;
tosihisa 0:e0ec137da369 418 buffer->argv[buffer->argc+0] = 0;
tosihisa 0:e0ec137da369 419 buffer->argv[buffer->argc+1] = -1;
tosihisa 0:e0ec137da369 420 buffer->argc++;
tosihisa 0:e0ec137da369 421
tosihisa 0:e0ec137da369 422 /* まず，NMEA センテンスをトークンで分解する．*/
tosihisa 0:e0ec137da369 423 /* 後で変換作業をしやすくするため */
tosihisa 0:e0ec137da369 424 for(i = 0;buffer->raw[i] != (unsigned char)('\0');i++){
tosihisa 0:e0ec137da369 425 if((buffer->raw[i] == (unsigned char)('*'))
tosihisa 0:e0ec137da369 426 || (buffer->raw[i] == (unsigned char)(0x0d))
tosihisa 0:e0ec137da369 427 || (buffer->raw[i] == (unsigned char)(0x0a))){
tosihisa 0:e0ec137da369 428 buffer->raw[i] = (unsigned char)('\0');
tosihisa 0:e0ec137da369 429 buffer->argv[buffer->argc] = -1;
tosihisa 0:e0ec137da369 430 break;
tosihisa 0:e0ec137da369 431 }
tosihisa 0:e0ec137da369 432 if(buffer->raw[i] == (unsigned char)(',')){
tosihisa 0:e0ec137da369 433 buffer->raw[i] = (unsigned char)('\0');
tosihisa 0:e0ec137da369 434 if(buffer->raw[i+1] != (unsigned char)('\0')){
tosihisa 0:e0ec137da369 435 buffer->argv[buffer->argc+0] = i+1;
tosihisa 0:e0ec137da369 436 buffer->argv[buffer->argc+1] = -1;
tosihisa 0:e0ec137da369 437 buffer->argc++;
tosihisa 0:e0ec137da369 438 if(buffer->argc >= LIBNMEA_NUMSOFARRAY(buffer->argv)){
tosihisa 0:e0ec137da369 439 return -1;
tosihisa 0:e0ec137da369 440 }
tosihisa 0:e0ec137da369 441 #ifdef DEBUG
tosihisa 0:e0ec137da369 442 if(buffer->argc > max_argc){
tosihisa 0:e0ec137da369 443 max_argc = buffer->argc;
tosihisa 0:e0ec137da369 444 }
tosihisa 0:e0ec137da369 445 #endif
tosihisa 0:e0ec137da369 446 }
tosihisa 0:e0ec137da369 447 }
tosihisa 0:e0ec137da369 448 }
tosihisa 0:e0ec137da369 449
tosihisa 0:e0ec137da369 450 tokerID = buffer->raw+buffer->argv[0];
tosihisa 0:e0ec137da369 451 if(*(tokerID+0) == (unsigned char)('$')){
tosihisa 0:e0ec137da369 452 if(*(tokerID+1) == (unsigned char)('P')){
tosihisa 0:e0ec137da369 453 /* P センテンスは無視する */
tosihisa 0:e0ec137da369 454 } else {
tosihisa 0:e0ec137da369 455 if((*(tokerID+1) == (unsigned char)('G'))
tosihisa 0:e0ec137da369 456 || (*(tokerID+2) == (unsigned char)('P'))){
tosihisa 0:e0ec137da369 457 /* GP for a GPS unit */
tosihisa 0:e0ec137da369 458 return _libNMEA_Parse1Line_GP(buffer,GPSinfo);
tosihisa 0:e0ec137da369 459 } else {
tosihisa 0:e0ec137da369 460 /* 不明なトーカーは無視する */
tosihisa 0:e0ec137da369 461 }
tosihisa 0:e0ec137da369 462 }
tosihisa 0:e0ec137da369 463 } else {
tosihisa 0:e0ec137da369 464 /* '$' から始まらないセンテンスは無視する */
tosihisa 0:e0ec137da369 465 }
tosihisa 0:e0ec137da369 466
tosihisa 0:e0ec137da369 467 return -2; /* このセンテンスは処理しませんでした */
tosihisa 0:e0ec137da369 468 }
tosihisa 0:e0ec137da369 469
tosihisa 0:e0ec137da369 470 #ifdef __SELFTEST__
tosihisa 0:e0ec137da369 471 #include <stdio.h>
tosihisa 0:e0ec137da369 472
tosihisa 0:e0ec137da369 473 char *sample[] = {
tosihisa 0:e0ec137da369 474 "$GPGGA,123519.00,4807.038247,N,01131.324523,E,1,08,0.9,545.42,M,46.93,M,5.0,1012*42\x0d\x0a",
tosihisa 0:e0ec137da369 475 "$GPGLL,4916.452349,N,12311.123215,W,225444.00,A,A*6A\x0d\x0a",
tosihisa 0:e0ec137da369 476 "$GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39\x0d\x0a",
tosihisa 0:e0ec137da369 477 "$GPGSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75\x0d\x0a",
tosihisa 0:e0ec137da369 478 "$GPRMC,225446.00,A,4916.452653,N,12311.123747,W,000.5,054.7,191194,06.2,W,A*68\x0d\x0a",
tosihisa 0:e0ec137da369 479 NULL
tosihisa 0:e0ec137da369 480 };
tosihisa 0:e0ec137da369 481
tosihisa 0:e0ec137da369 482 int main(int argc,char *argv[])
tosihisa 0:e0ec137da369 483 {
tosihisa 0:e0ec137da369 484 int i,j;
tosihisa 0:e0ec137da369 485 libNMEA_data_t sts;
tosihisa 0:e0ec137da369 486 int ret;
tosihisa 0:e0ec137da369 487 libNMEA_gps_info_t GPSinfo;
tosihisa 0:e0ec137da369 488 unsigned char *nextptr;
tosihisa 0:e0ec137da369 489 unsigned char *text;
tosihisa 0:e0ec137da369 490 libNMEA_realnum_t val;
tosihisa 0:e0ec137da369 491
tosihisa 0:e0ec137da369 492
tosihisa 0:e0ec137da369 493 /* x86 版 Linux だと，sizeof(libNMEA_gps_info_t) = 124 である．*/
tosihisa 0:e0ec137da369 494 printf("sizeof(libNMEA_gps_info_t) = %lu\n\n",sizeof(libNMEA_gps_info_t));
tosihisa 0:e0ec137da369 495 printf("sizeof(libNMEA_gps_time_t) = %lu\n\n",sizeof(libNMEA_gps_time_t));
tosihisa 0:e0ec137da369 496
tosihisa 0:e0ec137da369 497 text = "123.4567"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 498 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 499 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 500
tosihisa 0:e0ec137da369 501 text = "123.4567ABC"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 502 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 503 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 504
tosihisa 0:e0ec137da369 505 text = "9876"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 506 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 507 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 508
tosihisa 0:e0ec137da369 509 text = "98765ABC"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 510 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 511 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 512
tosihisa 0:e0ec137da369 513 text = "98765.123"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 514 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 515 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 516
tosihisa 0:e0ec137da369 517 text = "98765.00123"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 518 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 519 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 520
tosihisa 0:e0ec137da369 521 text = "1.0000123DEF"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 522 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 523 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 524
tosihisa 0:e0ec137da369 525 text = "1.000987XYZDEF"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 526 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 527 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 528
tosihisa 0:e0ec137da369 529 text = "12."; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 530 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 531 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 532
tosihisa 0:e0ec137da369 533 text = ".9876"; libNMEA_atod(text,&val,&nextptr);
tosihisa 0:e0ec137da369 534 printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
tosihisa 0:e0ec137da369 535 if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
tosihisa 0:e0ec137da369 536
tosihisa 0:e0ec137da369 537 for(i = 0;sample[i] != NULL;i++){
tosihisa 0:e0ec137da369 538 sts.cjobst = 0;
tosihisa 0:e0ec137da369 539 for(j = 0;*(sample[i]+j) != (char)('\0');j++){
tosihisa 0:e0ec137da369 540 ret = libNMEA_Parse1Char(*(sample[i]+j),&sts);
tosihisa 0:e0ec137da369 541 if(ret < 0){
tosihisa 0:e0ec137da369 542 printf("ret = %d (char:\'%c\')\n",ret,*(sample[i]+j));
tosihisa 0:e0ec137da369 543 }
tosihisa 0:e0ec137da369 544 if(ret == LIBNMEA_PARSE_COMPLETE){
tosihisa 0:e0ec137da369 545 printf("mes:[%s]\n",sample[i]);
tosihisa 0:e0ec137da369 546 printf("get:[%s]\n",sts.raw);
tosihisa 0:e0ec137da369 547 libNMEA_Parse1Line(&sts,&GPSinfo);
tosihisa 0:e0ec137da369 548 }
tosihisa 0:e0ec137da369 549 }
tosihisa 0:e0ec137da369 550 }
tosihisa 0:e0ec137da369 551 return 0;
tosihisa 0:e0ec137da369 552 }
tosihisa 0:e0ec137da369 553 #endif
tosihisa 0:e0ec137da369 554
tosihisa 0:e0ec137da369 555 #ifdef __SELFTEST2__
tosihisa 0:e0ec137da369 556 #include <stdio.h>
tosihisa 0:e0ec137da369 557 #include <unistd.h>
tosihisa 0:e0ec137da369 558
tosihisa 0:e0ec137da369 559 int main(int argc,char *argv[])
tosihisa 0:e0ec137da369 560 {
tosihisa 0:e0ec137da369 561 libNMEA_data_t sts;
tosihisa 0:e0ec137da369 562 int ret;
tosihisa 0:e0ec137da369 563 char buf;
tosihisa 0:e0ec137da369 564 libNMEA_gps_info_t GPSinfo;
tosihisa 0:e0ec137da369 565
tosihisa 0:e0ec137da369 566 sts.cjobst = 0;
tosihisa 0:e0ec137da369 567 while(read(fileno(stdin),&buf,sizeof(buf)) == sizeof(buf)){
tosihisa 0:e0ec137da369 568 ret = libNMEA_Parse1Char(buf,&sts);
tosihisa 0:e0ec137da369 569 if(ret < 0){
tosihisa 0:e0ec137da369 570 printf("ret = %d (char:\'%c\')\n",ret,buf);
tosihisa 0:e0ec137da369 571 printf("err:[%s]\n",sts.raw);
tosihisa 0:e0ec137da369 572 return 1;
tosihisa 0:e0ec137da369 573 }
tosihisa 0:e0ec137da369 574 if(ret == LIBNMEA_PARSE_SUMOK){
tosihisa 0:e0ec137da369 575 //printf("get:[%s]\n",sts.raw);
tosihisa 0:e0ec137da369 576 GPSinfo.valid.status = 0;
tosihisa 0:e0ec137da369 577 libNMEA_Parse1Line(&sts,&GPSinfo);
tosihisa 0:e0ec137da369 578 if(GPSinfo.valid.status != 0){
tosihisa 0:e0ec137da369 579 printf("lat:%c %8.5f\nlon:%c %8.5f\n",
tosihisa 0:e0ec137da369 580 GPSinfo.latlon.lat_unit,
tosihisa 0:e0ec137da369 581 GPSinfo.latlon.latitude,
tosihisa 0:e0ec137da369 582 GPSinfo.latlon.lon_unit,
tosihisa 0:e0ec137da369 583 GPSinfo.latlon.longitude );
tosihisa 0:e0ec137da369 584 } else {
tosihisa 0:e0ec137da369 585 //printf("lat:- ---.-----\nlon:- ---.-----\n");
tosihisa 0:e0ec137da369 586 }
tosihisa 0:e0ec137da369 587
tosihisa 0:e0ec137da369 588 sts.cjobst = 0;
tosihisa 0:e0ec137da369 589 }
tosihisa 0:e0ec137da369 590 }
tosihisa 0:e0ec137da369 591 printf("max_argc = %d\n",max_argc);
tosihisa 0:e0ec137da369 592 return 0;
tosihisa 0:e0ec137da369 593 }
tosihisa 0:e0ec137da369 594 #endif