Moog Crossbow NAV440 Series - Manual (Page 111)

NAV440 User Manual
7430‐0131‐01 Rev. F
Page 111
}
Pop(queue_ptr, numToPop);
if(Size(queue_ptr) <= 0)
{
/* header was not found */
return 0;
}
/* make sure we can read through minimum length packet */
if(Size(queue_ptr)<7)
{
return 0;
}
/* get data length (5th byte of packet) */
dataLength = peekByte(queue_ptr, 4);
/* make sure we can read through entire packet */
if(Size(queue_ptr) < 7+dataLength)
{
return 0;
}
/* check CRC */
myCRC = calcCRC(queue_ptr, 2,dataLength+3);
packetCRC = peekWord(queue_ptr, dataLength+5);
if(myCRC != packetCRC)
{
/* bad CRC on packet – remove the bad packet from the queue and return */
Pop(queue_ptr, dataLength+7);
return 0;
}
/* fill out result of parsing in structure */
result->packet_type = peekWord(queue_ptr, 2);
result->length = peekByte(queue_ptr, 4);
result->crc = packetCRC;
for(counter=0; counter < result->length; counter++)
{
result->data(counter) = peekByte(queue_ptr, 5+counter);
}
Pop(queue_ptr, dataLength+7);
return 1;
}
/*******************************************************************************
* FUNCTION: calcCRC calculates a 2-byte CRC on serial data using