File content as of revision 0:e0ec137da369:

#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. */
    /* １行を得た後ならば，再度初めから */
    /* パースエラーの後も，再度初めから */
    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;    /* チェックサム１文字目を得る */
            } else {
                buffer->sum = buffer->sum ^ c;    /* チェックサム計算 */
            }
            break;
        case 2:    /* チェックサム１文字目 */
            buffer->sumwk = _libNMEA_atoi(c);
            buffer->sumwk = buffer->sumwk << 4;
            cnewst = buffer->cjobst + 1;    /* チェックサム２文字目を得る */
            break;
        case 3:    /* チェックサム２文字目 */
            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