// Flush the current word buffer
// out to the phraseBuffer
static void flushWord(void){
uint8_t data;
size_t wordLen = wordBuffer.datalength;
// If the phrase cannot accept the entire
// word then say the current phrase first
if(bufferFreeSpace(&phraseBuffer) <= wordLen){
flushPhrase();
}
// Write the word to the phrase buffer
while(bufferGet(&wordBuffer,&data)){
bufferPut(&phraseBuffer,data);
}
}
static void handler(unsigned char c, void* device){
GPS_NMEA* gps = (GPS_NMEA*)device;
// Get working data
int msgIndex = gps->msgIndex;
boolean error = gps->error;
char* msg = gps->msg;
// If its a $ then its the start of a msg
if(c=='$'){
msgIndex = 0; // reset the msg buffer
error=FALSE; // clear error state
}
// Store the character
if(msgIndex < MAX_NMEA_MSG-1){
msg[msgIndex++] = c; // store character
msg[msgIndex] = 0; // null terminate string
}else{
error=TRUE; // the message is too long
}
// Test for end of line
if(c=='\n' && error==FALSE){
// Have got a line of length msgIndex
// Test for minimum size of '$GP...<,>.....*xx<cr><lf>'
if(msgIndex >= 8){
if(msg[0]=='$' && msg[1]=='G' && msg[2]=='P' && msg[6]==',' && msg[msgIndex-1]=='\n' && msg[msgIndex-2]=='\r' && msg[msgIndex-5]=='*'){
uint8_t myChecksum=0;
uint8_t i;
// Looks like a valid message
msg[msgIndex-2]=0; // chop off cr/lf
msgIndex=0;
// We only use GPRMC and GPGGA
if( msg[3]=='R' && msg[4]=='M' && msg[5]=='C') goto ok;
if( msg[3]=='G' && msg[4]=='G' && msg[5]=='A') goto ok;
goto end;
// Calculate the checksum
ok: for(i=1; ;){
c = msg[i++];
// Shouldn't happen - but check for end of string
if(c=='\0'){
goto end;
}
if(c=='*'){
break;
}
myChecksum ^= c;
}
// Remove checksum
msg[i-1]=0;
// String length including terminator
size_t msgLen = i;
if(bufferFreeSpace(&gps->msgQueueBuf) < msgLen){
// not enough space in buffer
goto end;
}
// Get received checksum
uint8_t rxChecksum=0;
c = msg[i++];
if(c>='0' && c<='9'){
rxChecksum += c-'0';
}else{
rxChecksum += c-'A'+10;
}
rxChecksum *= 16;
c = msg[i++];
if(c>='0' && c<='9'){
rxChecksum += c-'0';
}else{
rxChecksum += c-'A'+10;
}
// If checksums agree then add message to queue
if(rxChecksum==myChecksum){
if( ! bufferQueue(&gps->msgQueueBuf, msg, msgLen) ){
goto end; // couldn't add it for some reason
}
}else{
#ifdef DEBUG
PRINTF(stdout,"Bad checksum:");
#endif
}
}
}
}
end:
// Set working data
gps->msgIndex = msgIndex;
gps->error = error;
}