nmeap01.c

GARMIN的GPS信息解释器

    return id;
}

/**
                 +-5-+              +---+
                 v   |              v   |
 +------+       +------+           +------+         +------+         +------+
 |   0  |--$--> |1-hdr |--alnum--> |2-data|----\r-->| 6-LF |---\n--->| done |--> 0
 +------+       +------+           +------+         +------+         +------+
                                    |                 ^
                                    *                 +--------\r-------+
                                    V                                   |
                                 +------+           +------+         +------+
                                 |3-cks |--xdigit-->|4-cks |-xdigit->| 5-CR |
                                 +------+           +------+         +------+
                                 
return to start conditions:
1. buffer overflow
2. invalid character for state

checksum calculation
two hex digits represent the XOR of all characters between, but not
including, the "$" and "*".  A checksum is required on some
sentences.

*/
int nmeap_parse(nmeap_context_t *context,char ch)
{
    int status = 0;
    
    /* check for input buffer overrun first to avoid duplicating code in the
    individual states
    */
    if (context->input_count >= (sizeof(context->input)-1)) {
        /* input buffer overrun, restart state machine */
        context->input_state = 0;
        /* reset input count */
        context->input_count = 0;
    }
    
    /* store the byte */
    context->input[context->input_count] = ch;
    
    /* next buffer position */
    context->input_count++;
    
    /* run it through the lexical scanner */
    switch(context->input_state) {
    /* LOOKING FOR $ */
    case 0:
        if (ch == '$') {
            /*look for id */
            context->input_state = 1;
            context->ccks        = 0;
			context->icks        = 0;
        }
        else {
            /* header error, start over */
            context->err_hdr++;
            context->input_state = 0;
            context->input_count = 0;
        }
        break;
    /* LOOKING FOR 5 CHARACTER SENTENCE ID */
    case 1:
        /* allow numbers even though it isn't usually done */
        /* a proprietary id might have a numeral */
        if (isalnum(ch)) {
            /* store name separately */
            context->input_name[context->input_count - 2] = ch;
            /* checksum */
            context->ccks ^= ch;
            /* end of header? */
            if (context->input_count >= 6) {
                /* yes, get body */
                context->input_state = 2;
            }
        }
        else {
            /* bad character, start over */
            context->err_id++;
            context->input_state = 0;
            context->input_count = 0;
        }
        break;
    /* LOOKING FOR CR OR CHECKSUM INDICATOR */
    case 2:
        if (ch == '*') {
            /* this sentence has a checksum */
            context->input_state = 3;
        }
        else if (ch == '\r') {
            /* carriage return, no checksum, force a match */
			context->icks = 0;
			context->ccks = 0;
            context->input_state = 6;
        }
        else {
            /* continue accumulating data */
            /* checksum */
            context->ccks ^= ch;
        }
        break;
    /* LOOKING FOR FIRST CHECKSUM CHARACTER */
    case 3:
        /* must be upper case hex digit */
        if (isxdigit(ch) && (ch <= 'F')) {
            /* got first checksum byte */
            context->input_state = 4;
            context->icks = HEXTOBIN(ch) << 4;
        }
        else {
            /* input error, restart */
            context->err_cks++;
            context->input_state = 0;
            context->input_count = 0;
        }
        break;
        /* LOOKING FOR SECOND CHECKSUM CHARACTER */
    case 4:
        /* must be upper case hex digit */
        if (isxdigit(ch) && (ch <= 'F')) {
            /* got second checksum byte */
            context->input_state = 5;
            context->icks += HEXTOBIN(ch);
        }
        else {
            /* input error, restart */
            context->err_cks++;
            context->input_state = 0;
            context->input_count = 0;
        }
        break;
    /* LOOKING FOR CR */
    case 5:
        if (ch == '\r') {
            /* carriage return */
            context->input_state = 6;
        }
        else {
            /* input error, restart */
            context->err_crl++;
            context->input_state = 0;
            context->input_count = 0;
        }
        break;
    /* LOOKING FOR LINE FEED */
    case 6:
        if (ch == '\n') {
            /* linefeed, line complete */
            
            /* delimit buffer */
            context->input[context->input_count] = 0;
            
            /* if the checksums match, process the sentence */
			if (context->ccks == context->icks) {
				/* process */
				status = nmeap_process(context);
				
				/* count good messages */
				context->msgs++;
			}
			else {
				/* count checksum errors */
				context->err_cks++;
			}
            
            /* restart next time */
            context->input_state = 0;
            context->input_count = 0;
        }
        else {
            /* input error, restart */
            context->err_crl++;
            context->input_state = 0;
            context->input_count = 0;
        }
        break;
    default:
        context->err_unk++;
        context->input_state = 0;
        break;
    }
            
	return status;
}

/** 
 * parse a buffer of nmea data
 */
int nmeap_parseBuffer(nmeap_context_t *context,const char *buffer,int *length)
{
    int  i;
    int  status;
    int  rem;
    int  tlen;
    
    tlen   = *length;
    rem    = *length;
    status = 0;
    /* for each byte in the buffer */
    for(i=0;i<tlen;i++) {
        /* decrement remaining byte count */
        rem--;
        /* parse the byte */
        status = nmeap_parse(context,buffer[i]);
        if (status != 0) {
            /* message found or error */
            break;
        }
    }
	
    /* return remaining byte count */
    *length = rem;
	
    return status;
}

/** 
 * standard GPGGA sentence parser 
 */
int nmeap_gpgga(nmeap_context_t *context,nmeap_sentence_t *sentence)
{
#ifndef NDEBUG	
    int i;
#endif
    
    /* get pointer to sentence data */
    nmeap_gga_t *gga = (nmeap_gga_t *)sentence->data;
    
    /* if there is a data element, extract data from the tokens */
	if (gga != 0) {
		gga->latitude  = nmeap_latitude(context->token[2],context->token[3]);
		gga->longitude = nmeap_longitude(context->token[4],context->token[5]);
		gga->altitude  = nmeap_altitude(context->token[9],context->token[10]);
		gga->time       = atoi(context->token[1]);
		gga->satellites = atoi(context->token[7]);
		gga->quality    = atoi(context->token[6]);
		gga->hdop       = atof(context->token[8]);
		gga->geoid      = nmeap_altitude(context->token[11],context->token[12]);
	}

#ifndef NDEBUG    
    /* print raw input string */
    printf("%s",context->debug_input);
    
    /* print some validation data */
    printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
    
    /* print the tokens */
    for(i=0;i<context->tokens;i++) {
        printf("%d:%s\n",i,context->token[i]);
    }
#endif   

    /* if the sentence has a callout, call it */
    if (sentence->callout != 0) {
        (*sentence->callout)(context,gga,context->user_data);
    }
    
    return NMEAP_GPGGA;
}

/** 
 * standard GPRMCntence parser 
 */
int nmeap_gprmc(nmeap_context_t *context,nmeap_sentence_t *sentence)
{
#ifndef NDEBUG	
    int i;
#endif

    /* get pointer to sentence data */
    nmeap_rmc_t *rmc = (nmeap_rmc_t *)sentence->data;
    
	/* if there is a data element, use it */
	if (rmc != 0) {
		/* extract data from the tokens */
		rmc->time       = atoi(context->token[1]);
		rmc->warn       = *context->token[2];
		rmc->latitude  = nmeap_latitude(context->token[3],context->token[4]);
		rmc->longitude = nmeap_longitude(context->token[5],context->token[6]);
		rmc->speed      = atof(context->token[7]);
		rmc->course     = atof(context->token[8]);
		rmc->date       = atoi(context->token[9]);
		rmc->magvar     = atof(context->token[10]);
	}

#ifndef NDEBUG    
    /* print raw input string */
    printf("%s",context->debug_input);
    
    /* print some validation data */
    printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
    
    /* print the tokens */
    for(i=0;i<context->tokens;i++) {
        printf("%d:%s\n",i,context->token[i]);
    }
#endif   

    /* if the sentence has a callout, call it */
    if (sentence->callout != 0) {
        (*sentence->callout)(context,rmc,context->user_data);
    }
    
    return NMEAP_GPRMC;
}