Toshihisa T
/
S1315FLogger
Skytraq S1315F-RAW-EVK Logger
libT/portable/NMEA_parse.c
- Committer:
- tosihisa
- Date:
- 2010-12-19
- Revision:
- 2:7eb11afe02bd
- Parent:
- 0:e0ec137da369
File content as of revision 2:7eb11afe02bd:
#include "NMEA_parse.h" #ifndef NULL # define NULL ((void *)0) #endif /* nmealib GM-316 FV-M11 GGA Global Positioning System Fix Data. O O O GSA GNSS DOP and Active Satellites O O O GSV GNSS Satellites in View O O O RMC Recommended Minimum Navigation Information O O O VTG Course Over Ground and Ground Speed O O O GGL Geographic Position – Latitude / Longitude X O O ZDA Time & Date X O(synchronized to PPS) O */ static unsigned char _libNMEA_atoi(unsigned char c) { if((c >= (unsigned char)('0')) && (c <= (unsigned char)('9'))) return (c - (unsigned char)('0')); if((c >= (unsigned char)('A')) && (c <= (unsigned char)('F'))) return (c - (unsigned char)('A')) + 10; return (c - (unsigned char)('a')) + 10; } short libNMEA_Parse1Char(unsigned char c,libNMEA_data_t *buffer) { register short cnewst = 0; /* It does again at the time of beginning after one line is obtained it. */ /* It does again after the syntax error at the time of beginning. */ /* 1行を得た後ならば,再度初めから */ /* パースエラーの後も,再度初めから */ if((buffer->cjobst >= LIBNMEA_PARSE_COMPLETE) || (buffer->cjobst < LIBNMEA_PARSE_NONE)){ buffer->cjobst = LIBNMEA_PARSE_NONE; } if(buffer->cjobst == LIBNMEA_PARSE_NONE){ /* Wait '$' */ if((c != (unsigned char)('$')) && (c != (unsigned char)('!'))){ return buffer->cjobst; } buffer->rawLength = 0; buffer->sum = 0; } /* The size check is previously done because it stores it here in the buffer now. */ if(buffer->rawLength >= (sizeof(buffer->raw)-2)){ buffer->cjobst = -3; /* Buffer size overflow */ return buffer->cjobst; } buffer->raw[buffer->rawLength] = c; buffer->rawLength += 1; buffer->raw[buffer->rawLength] = (unsigned char)('\0'); cnewst = -1; /* Syntax error (It initializes it by the default value.) */ switch(buffer->cjobst){ case LIBNMEA_PARSE_NONE: cnewst = 1; break; case 1: /* データ待ち */ cnewst = buffer->cjobst; if(c == (unsigned char)('*')){ cnewst += 1; /* チェックサム1文字目を得る */ } else { buffer->sum = buffer->sum ^ c; /* チェックサム計算 */ } break; case 2: /* チェックサム1文字目 */ buffer->sumwk = _libNMEA_atoi(c); buffer->sumwk = buffer->sumwk << 4; cnewst = buffer->cjobst + 1; /* チェックサム2文字目を得る */ break; case 3: /* チェックサム2文字目 */ if((buffer->sumwk |= _libNMEA_atoi(c)) == buffer->sum){ cnewst = LIBNMEA_PARSE_SUMOK; /* CR待ち */ } else { //printf("SUM:%02x/%02x\n",buffer->sumwk,buffer->sum); cnewst = -2; /* チェックサムエラー */ } break; case LIBNMEA_PARSE_SUMOK: /* CR 待ち */ if(c == (unsigned char)0x0d){ cnewst = buffer->cjobst + 1; /* LF待ち */ } break; case LIBNMEA_PARSE_SUMOK+1: /* LF 待ち */ if(c == (unsigned char)0x0a){ cnewst = LIBNMEA_PARSE_COMPLETE; /* センテンス完了 */ } break; } buffer->cjobst = cnewst; return buffer->cjobst; } /* 10進数固定 */ short libNMEA_atol(unsigned char *numstr,long *val,long *length,unsigned char **nextptr) { long retval = 0; int i = 0; int mi = 0; unsigned char c; *length = 1; if(*(numstr + i) == (unsigned char)('-')){ mi = 1; i++; } for(;*(numstr + i) != (unsigned char)('\0');i++){ c = *(numstr + i); if((c >= (unsigned char)('0')) && (c <= (unsigned char)('9'))){ c = c - (unsigned char)('0'); retval = (retval * 10) + c; *length = *length * 10; } else { break; } } if((nextptr != NULL) && (*nextptr != NULL)){ *nextptr = numstr + i; } *val = (mi) ? (0-retval) : retval; return 0; } short libNMEA_atod(unsigned char *numstr,libNMEA_realnum_t *val,unsigned char **nextptr) { long lval = 0; libNMEA_realnum_t e = 0.0; libNMEA_realnum_t f = 0.0; unsigned char *nptr; long length; /* TODO:符号チェックを入れる事 */ /* 整数部 */ libNMEA_atol(numstr,&lval,&length,&nptr); e = (libNMEA_realnum_t)lval; /* 小数部 */ if(*nptr == (unsigned char)('.')){ nptr++; libNMEA_atol(nptr,&lval,&length,&nptr); f = ((libNMEA_realnum_t)lval) / ((libNMEA_realnum_t)length); } if((nextptr != NULL) && (*nextptr != NULL)){ *nextptr = nptr; } *val = e + f; return 0; } /* GPS の緯度経度 ddmm.mmmm(dddmm.mmmm) 形式の文字列を, dd.dddd の libNMEA_realnum_t 形式に変換する. */ /* http://www.circuitcellar.com/library/print/1000/Stefan123/4.htm */ short libNMEA_atodeg(unsigned char *numstr,libNMEA_realnum_t *val) { volatile long wk = 0; volatile long deg_l = 0; long length = 0; libNMEA_realnum_t mm_d = 0; unsigned char *nextptr; /* ddmm 部を取り出す */ libNMEA_atol(numstr,(long *)&wk,&length,&nextptr); /* dd を得る */ deg_l = wk / 100; /* .mmmm 部を取り出す */ libNMEA_atod(nextptr,&mm_d,&nextptr); /* mm.mmmm にする */ mm_d = ((libNMEA_realnum_t)(wk % 100)) + mm_d; /* 0.dddd に変換する */ mm_d = (mm_d / 60.0); /* もし,mm_d が 1.0 よりも大きい場合,mm.mmmm は60分を超える値であり, GPS の緯度経度値として適切ではない */ if(mm_d > 1.0){ return -1; } *val = ((libNMEA_realnum_t)deg_l) + mm_d; /* dd.dddd 形式に変換 */ return 0; } #if 0 static short _libNMEA_print_args(libNMEA_data_t *buffer) { int i; extern int printf(const char *format, ...); printf("ARGC %5d:",buffer->argc); for(i=0;i < buffer->argc;i++){ printf("[%s]",buffer->raw + buffer->argv[i]); } printf("\n"); return 0; } #endif static void _libNMEA_String2Time(unsigned char *numstr,libNMEA_gps_info_t *GPSinfo) { long wk; long length; unsigned char *nextptr; libNMEA_atol(numstr,&wk,&length,&nextptr); GPSinfo->time.sec = wk % 100; wk = wk/100; GPSinfo->time.min = wk % 100; wk = wk/100; GPSinfo->time.hour = wk % 100; GPSinfo->time.msec = 0; if((nextptr != NULL) && (*nextptr == (unsigned char)('.'))){ libNMEA_atol(nextptr+1,&wk,&length,(unsigned char **)NULL); GPSinfo->time.msec = wk; } } static void _libNMEA_setInvalid(libNMEA_gps_info_t *GPSinfo) { GPSinfo->valid.status = 0; GPSinfo->valid.positioning = 0; GPSinfo->valid.active = 0; } /* Recommended Minimum Course Response Message */ static short _libNMEA_Parse1Line_GPRMC(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo) { unsigned char *nextptr; if(buffer->argc != 13){ return -2; /* データの数がおかしい */ } //_libNMEA_print_args(buffer); /* まず,真っ先に見るのは,GPS データのステータス(妥当性) */ if(*(buffer->raw+buffer->argv[2]+0) != (unsigned char)('A')){ /* GPS データ妥当性無し */ GPSinfo->valid.status = 0; /* 妥当性が無いので,これ以上解析する必要は無いので終わる */ return 0; } /* 測位状態を得る */ switch(*(buffer->raw+buffer->argv[12]+0)){ case (unsigned char)('A'): /* 単独測位 */ case (unsigned char)('D'): /* DGPS */ GPSinfo->status.positioning = *(buffer->raw+buffer->argv[12]+0); GPSinfo->valid.positioning = 1; break; default: GPSinfo->status.positioning = (unsigned char)('N'); _libNMEA_setInvalid(GPSinfo); return 0; /* 無効な測位状態なので返る */ } /* *** 妥当性のあるデータ *** */ /* 緯度を得る */ libNMEA_atodeg( buffer->raw+buffer->argv[3] ,&(GPSinfo->latlon.latitude) ); GPSinfo->latlon.lat_unit = *(buffer->raw+buffer->argv[4]); GPSinfo->valid.latitude = 1; /* 経度を得る */ libNMEA_atodeg( buffer->raw+buffer->argv[5] ,&(GPSinfo->latlon.longitude ) ); GPSinfo->latlon.lon_unit = *(buffer->raw+buffer->argv[6]); GPSinfo->valid.longitude = 1; /* 対地速度(ノット)を得る */ libNMEA_atod( buffer->raw+buffer->argv[7], &(GPSinfo->speed.ground), &nextptr); GPSinfo->valid.speed = 1; /* 進行方向(度,真北)を得る */ libNMEA_atod( buffer->raw+buffer->argv[8], &(GPSinfo->direction.trueNorth.deg), &nextptr); GPSinfo->valid.direction = 1; GPSinfo->valid.timeRMC = 0; if(1){ /* 現在時刻/日付を得る.*/ long wk; long length; _libNMEA_String2Time(buffer->raw+buffer->argv[1],GPSinfo); libNMEA_atol(buffer->raw+buffer->argv[9],&wk,&length,&nextptr); /* GPRMC は,西暦は下2桁しかないので,2000を加算する. */ GPSinfo->date.year = (wk % 100) + 2000; /* after year 2000. */ wk = wk/100; GPSinfo->date.mon = wk % 100; wk = wk/100; GPSinfo->date.day = wk % 100; GPSinfo->valid.timeRMC = 1; } /* ステータスは有効 */ GPSinfo->valid.status = 1; return 0; } static short _libNMEA_Parse1Line_GPZDA(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo) { long wk; long length; unsigned char *nextptr; if(buffer->argc != 7){ return -2; } GPSinfo->valid.timeZDA = 0; _libNMEA_String2Time(buffer->raw+buffer->argv[1],GPSinfo); libNMEA_atol(buffer->raw+buffer->argv[2],&wk,&length,&nextptr); GPSinfo->date.day = wk; libNMEA_atol(buffer->raw+buffer->argv[3],&wk,&length,&nextptr); GPSinfo->date.mon = wk; libNMEA_atol(buffer->raw+buffer->argv[4],&wk,&length,&nextptr); GPSinfo->date.year = wk; GPSinfo->valid.timeZDA = 1; return 0; } /* GPGSA - GNSS DOP and Active Satellites */ /* 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 */ static short _libNMEA_Parse1Line_GPGSA(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo) { if(buffer->argc != 18){ return -2; } switch( *(buffer->raw+buffer->argv[2]) ){ case '2': /* 2D */ case '3': /* 3D */ GPSinfo->status.active = *(buffer->raw+buffer->argv[2]); GPSinfo->valid.active = 1; break; default: /* Fix not available */ _libNMEA_setInvalid(GPSinfo); return 0; /* 無効な測位状態なので返る */ } GPSinfo->valid.accuracy_pdop = 0; libNMEA_atod( buffer->raw+buffer->argv[15], &(GPSinfo->accuracy.p.dop), (unsigned char **)NULL); GPSinfo->valid.accuracy_pdop = 1; GPSinfo->valid.accuracy_hdop = 0; libNMEA_atod( buffer->raw+buffer->argv[16], &(GPSinfo->accuracy.h.dop), (unsigned char **)NULL); GPSinfo->valid.accuracy_hdop = 1; GPSinfo->valid.accuracy_vdop = 0; libNMEA_atod( buffer->raw+buffer->argv[17], &(GPSinfo->accuracy.v.dop), (unsigned char **)NULL); GPSinfo->valid.accuracy_vdop = 1; return 0; } static short _libNMEA_Parse1Line_GP(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo) { unsigned char *sentenceID = (unsigned char *)0; sentenceID = buffer->raw+buffer->argv[0]; sentenceID += 3; /* "$GP" をスキップ */ if( (*(sentenceID+0) == (unsigned char)('R')) && (*(sentenceID+1) == (unsigned char)('M')) && (*(sentenceID+2) == (unsigned char)('C'))){ return _libNMEA_Parse1Line_GPRMC(buffer,GPSinfo); } else if( (*(sentenceID+0) == (unsigned char)('Z')) && (*(sentenceID+1) == (unsigned char)('D')) && (*(sentenceID+2) == (unsigned char)('A'))){ return _libNMEA_Parse1Line_GPZDA(buffer,GPSinfo); } else if((*(sentenceID+0) == (unsigned char)('G')) && (*(sentenceID+1) == (unsigned char)('S')) && (*(sentenceID+2) == (unsigned char)('A'))){ return _libNMEA_Parse1Line_GPGSA(buffer,GPSinfo); } return -2; /* このセンテンスは処理しませんでした */ } #ifdef DEBUG /* { */ static short max_argc = 0; #endif /* } */ short libNMEA_Parse1Line(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo) { int i; unsigned char *tokerID = (unsigned char *)0; buffer->argc = 0; buffer->argv[buffer->argc+0] = 0; buffer->argv[buffer->argc+1] = -1; buffer->argc++; /* まず,NMEA センテンスをトークンで分解する.*/ /* 後で変換作業をしやすくするため */ for(i = 0;buffer->raw[i] != (unsigned char)('\0');i++){ if((buffer->raw[i] == (unsigned char)('*')) || (buffer->raw[i] == (unsigned char)(0x0d)) || (buffer->raw[i] == (unsigned char)(0x0a))){ buffer->raw[i] = (unsigned char)('\0'); buffer->argv[buffer->argc] = -1; break; } if(buffer->raw[i] == (unsigned char)(',')){ buffer->raw[i] = (unsigned char)('\0'); if(buffer->raw[i+1] != (unsigned char)('\0')){ buffer->argv[buffer->argc+0] = i+1; buffer->argv[buffer->argc+1] = -1; buffer->argc++; if(buffer->argc >= LIBNMEA_NUMSOFARRAY(buffer->argv)){ return -1; } #ifdef DEBUG if(buffer->argc > max_argc){ max_argc = buffer->argc; } #endif } } } tokerID = buffer->raw+buffer->argv[0]; if(*(tokerID+0) == (unsigned char)('$')){ if(*(tokerID+1) == (unsigned char)('P')){ /* P センテンスは無視する */ } else { if((*(tokerID+1) == (unsigned char)('G')) || (*(tokerID+2) == (unsigned char)('P'))){ /* GP for a GPS unit */ return _libNMEA_Parse1Line_GP(buffer,GPSinfo); } else { /* 不明なトーカーは無視する */ } } } else { /* '$' から始まらないセンテンスは無視する */ } return -2; /* このセンテンスは処理しませんでした */ } #ifdef __SELFTEST__ #include <stdio.h> char *sample[] = { "$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", "$GPGLL,4916.452349,N,12311.123215,W,225444.00,A,A*6A\x0d\x0a", "$GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39\x0d\x0a", "$GPGSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75\x0d\x0a", "$GPRMC,225446.00,A,4916.452653,N,12311.123747,W,000.5,054.7,191194,06.2,W,A*68\x0d\x0a", NULL }; int main(int argc,char *argv[]) { int i,j; libNMEA_data_t sts; int ret; libNMEA_gps_info_t GPSinfo; unsigned char *nextptr; unsigned char *text; libNMEA_realnum_t val; /* x86 版 Linux だと,sizeof(libNMEA_gps_info_t) = 124 である.*/ printf("sizeof(libNMEA_gps_info_t) = %lu\n\n",sizeof(libNMEA_gps_info_t)); printf("sizeof(libNMEA_gps_time_t) = %lu\n\n",sizeof(libNMEA_gps_time_t)); text = "123.4567"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "123.4567ABC"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "9876"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "98765ABC"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "98765.123"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "98765.00123"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "1.0000123DEF"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "1.000987XYZDEF"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = "12."; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } text = ".9876"; libNMEA_atod(text,&val,&nextptr); printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr); if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); } for(i = 0;sample[i] != NULL;i++){ sts.cjobst = 0; for(j = 0;*(sample[i]+j) != (char)('\0');j++){ ret = libNMEA_Parse1Char(*(sample[i]+j),&sts); if(ret < 0){ printf("ret = %d (char:\'%c\')\n",ret,*(sample[i]+j)); } if(ret == LIBNMEA_PARSE_COMPLETE){ printf("mes:[%s]\n",sample[i]); printf("get:[%s]\n",sts.raw); libNMEA_Parse1Line(&sts,&GPSinfo); } } } return 0; } #endif #ifdef __SELFTEST2__ #include <stdio.h> #include <unistd.h> int main(int argc,char *argv[]) { libNMEA_data_t sts; int ret; char buf; libNMEA_gps_info_t GPSinfo; sts.cjobst = 0; while(read(fileno(stdin),&buf,sizeof(buf)) == sizeof(buf)){ ret = libNMEA_Parse1Char(buf,&sts); if(ret < 0){ printf("ret = %d (char:\'%c\')\n",ret,buf); printf("err:[%s]\n",sts.raw); return 1; } if(ret == LIBNMEA_PARSE_SUMOK){ //printf("get:[%s]\n",sts.raw); GPSinfo.valid.status = 0; libNMEA_Parse1Line(&sts,&GPSinfo); if(GPSinfo.valid.status != 0){ printf("lat:%c %8.5f\nlon:%c %8.5f\n", GPSinfo.latlon.lat_unit, GPSinfo.latlon.latitude, GPSinfo.latlon.lon_unit, GPSinfo.latlon.longitude ); } else { //printf("lat:- ---.-----\nlon:- ---.-----\n"); } sts.cjobst = 0; } } printf("max_argc = %d\n",max_argc); return 0; } #endif