/* == operator */ bool SFPacket::operator==(SFPacket const& pPacket) { bool retval=false; if((pPacket.getType() == type) && (pPacket.getLength() == length) && (pPacket.getSeqno() == seqno)) { if((type == SF_PACKET_ACK) || (type == SF_PACKET_NO_ACK)) { retval = (memcmp(pPacket.getPayload(), getPayload(), length) == 0); } } return retval; }
// copy constructor SFPacket::SFPacket(const SFPacket &pPacket) { length = pPacket.getLength(); type = pPacket.getType(); seqno = pPacket.getSeqno(); setPayload(pPacket.getPayload(), length); }
/* reads packet */ bool SerialComm::readPacket(SFPacket &pPacket) { bool sync = false; bool escape = false; bool completePacket = false; int count = 0; uint16_t crc = 0; char buffer[maxMTU]; while(!completePacket) { buffer[count] = nextRaw(); if(sync && (count == 1) && (buffer[count] == SYNC_BYTE)) { DEBUG("SerialComm::readPacket double sync byte"); sync = false; escape = false; count = 1; crc = 0; buffer[0] = SYNC_BYTE; } if (!sync) { // wait for sync if (buffer[0] == SYNC_BYTE) { sync = true; escape = false; count = 1; crc = 0; } } 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); } ++count; escape = false; } } else if (buffer[count] == ESCAPE_BYTE) { // next byte is escaped escape = true; } else if (buffer[count] == SYNC_BYTE) { // calculate last crc byte if (count > 3) { crc = SerialComm::byteCRC(buffer[count-3], crc); } uint16_t packetCRC = (buffer[count - 2] & 0xff) | ((buffer[count - 1] << 8) & 0xff00); if (count < minMTU) { DEBUG("SerialComm::readPacket : frame too short - size = " << count << " : resynchronising ") sync = false; escape = false; count = crc = 0; badPacketCount++; } else if (crc != packetCRC) { DEBUG("SerialComm::readPacket : bad crc - calculated crc = " << crc << " packet crc = " << packetCRC << " : resynchronising " ) sync = false; escape = false; count = crc = 0; badPacketCount++; } else { pPacket.setType(buffer[typeOffset]); pPacket.setSeqno(buffer[seqnoOffset]); switch (buffer[typeOffset]) { case SF_ACK: break; case SF_PACKET_NO_ACK: case SF_PACKET_ACK: // buffer / payload // FIXME: strange packet format!? because seqno is not really defined - missing :( pPacket.setPayload(&buffer[payloadOffset]-1, count+1+1 - serialHeaderBytes); break; default: DEBUG("SerialComm::readPacket : unknown packet type = " << static_cast<uint16_t>(buffer[typeOffset] & 0xff)) ; } completePacket = true; #ifdef DEBUG_RAW_SERIALCOMM DEBUG("SerialComm::readPacket : raw data >>") for (int j=0; j <= count; j++) { cout << std::hex << static_cast<uint16_t>(buffer[j] & 0xff) << " " << std::dec; } cout << endl; cout << "as payload >> " << endl; const char* ptr = pPacket.getPayload(); for (int j=0; j < pPacket.getLength(); j++) { cout << std::hex << static_cast<uint16_t>(ptr[j] & 0xff) << " " << std::dec; } cout << endl; #endif } } else { if (count > 3) { crc = SerialComm::byteCRC(buffer[count-3], crc); } ++count; } }