- else if (count >= maxMTU)
- {
- DEBUG("SerialComm::readPacket : frame too long - size = " << count << " : resynchronising")
- sync = false;
- escape = false;
- count = crc = 0;
- badPacketCount++;
- }
- else if (escape)
- {
- if (buffer[count] == SYNC_BYTE)
- {
- DEBUG("SerialComm::readPacket : resynchronising")
- sync = false;
- escape = false;
- count = crc = 0;
- badPacketCount++;
- }
- else
- {
- buffer[count] ^= 0x20;
- if (count > 3)
- {
- crc = SerialComm::byteCRC(buffer[count-3], crc);
+ else if(state == IN_SYNC) {
+ if(nextByte == SYNC_BYTE) {
+ if(count < minMTU) {
+ DEBUG("SerialComm::readPacket : frame too short - size = " << count << " : resynchronising ");
+ badPacketCount++;
+ count = 0;
+ }
+ else {
+ if(checkCrc(buffer, count)) {
+ pPacket.setType(buffer[typeOffset]);
+ pPacket.setSeqno(buffer[seqnoOffset]);
+ switch (buffer[typeOffset]) {
+ case SF_ACK:
+ break;
+ case SF_PACKET_NO_ACK:
+ case SF_PACKET_ACK:
+ pPacket.setPayload((char *)(&buffer[payloadOffset]-1), count+1+1 - serialHeaderBytes);
+ break;
+ default:
+ DEBUG("SerialComm::readPacket : unknown packet type = " \
+ << static_cast<uint16_t>(buffer[typeOffset] & 0xff));
+ break;
+ }
+ break; // leave loop
+ }
+ else {
+ DEBUG("SerialComm::readPacket : bad crc");
+ count = 0;
+ badPacketCount++;
+ }