Toshihisa T / Mbed 2 deprecated ENVLogger

Dependencies:   ChaNFSSD mbed BMP085 SHT2x

libT/portable/NMEA_parse.c@9:9ca3db7ed7cb, 2012-02-27 (annotated)

Committer:
tosihisa
Date:
Mon Feb 27 16:20:15 2012 +0000
Revision:
9:9ca3db7ed7cb
Parent:
1:83960ee4d9a2 
V0.89.2. GPS recv data is OK.

Who changed what in which revision?

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