/* == 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;
}
}
