void interpreter(void) {
infof(ME "interpreter()\n");
while(1) {
int addr = rx();
if (addr) {
// not for us, just drop it.
int count = rx();
while (count--) rx();
}
else {
interpret_packet();
}
}
}
int NmeaParser::receiveData(const uint8 *indata, int len)
{
char databyte; /* Byte being examined */
int numDecodedPackets = 0;
if (state == state_pre_system_startup) {
#ifdef DRIV_NMEA_DEBUG_ALL_INPUT
m_log->debug("Dropped data since not initialized: %i bytes", len);
#endif
return -1;
}
while (len) {
databyte=*indata;
#ifdef DRIV_NMEA_DEBUG_ALL_INPUT
m_log->debug("rd: 0x%08x, 0x%02x", int(state), databyte);
#endif
switch (state) {
case state_idle:
if (databyte=='$') {
state=state_in_packet;
reassembly_ptr = reassmebled_pkt;
calculated_checksum = 0;
}
break;
case state_in_packet_after_star:
/* Calculate the checksum */
if (databyte == '\r' || databyte == '\n') {
// Checksum done, handle the packet
++numDecodedPackets;
if ( !checksum_present || (checksum_from_pkt == calculated_checksum) ) {
interpret_packet();
} else {
DEBUG_BAD_PACKETS("Broken nmea checksum, %02x!=%02x\n", checksum_from_pkt, calculated_checksum);
}
state=state_idle;
} else {
checksum_from_pkt = (checksum_from_pkt << 4) + hexToInt(databyte);
checksum_present=1;
}
break;
case state_in_packet:
if (databyte=='*') {
state=state_in_packet_after_star;
checksum_from_pkt = 0;
checksum_present=0;
break;
}
if ((reassembly_ptr - reassmebled_pkt) >=
int(sizeof(reassmebled_pkt)) ) {
state=state_idle; /* Oversized packet, drop it */
DEBUG_BAD_PACKETS("%s","Oversized packet!\n");
break;
}
*(reassembly_ptr++)=databyte;
calculated_checksum ^= databyte;
break;
case state_pre_system_startup:
m_log->error("receiveData: state should never be pre_system_startup");
return 0; //XXX dom something meningful here.
}
indata++;
len--;
}
return numDecodedPackets;
}