// Caller is resonsible for the received_data buffer's appropriate size.
// Returns the total numberSet bytes received for packet.
// If receive timed out, value of num_bytes_received is undefined.
bool RXPacket(uint8_t* received_data, int packet_length, int* num_bytes_received) {
dxl_hal_set_timeout(packet_length);
// Repeatedly attempt to read until either we have all of the bytes
// requested or the timeout expires.
// TODO replace this logic with something that isn't so CPU intensive.
// like interrupt driven...
int bytes_read = 0;
while (bytes_read < packet_length) {
if (dxl_hal_timeout() == 1) {
return false;
}
assert(packet_length < MAX_PACKET_BYTES);
bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length);
}
// PrintPacket(data, num_bytes);
// Now attempt to match the header to the data received.
int header_match_count = 0;
int buffer_index = 0;
while (header_match_count < HEADER_SIZE) {
if (buffer_index >= bytes_read) {
return false;
}
if (received_data[buffer_index++] == HEADER[header_match_count]) {
header_match_count++;
} else {
header_match_count = 0;
}
}
// Now, the header is matched
// Move bytes down to the beginning of the data buffer.
int packet_start_index = buffer_index - HEADER_SIZE;
bytes_read -= packet_start_index;
memmove(received_data, &(received_data[packet_start_index]), bytes_read);
// Now read the remaining bytes if necessary.
while (bytes_read < packet_length) {
assert((packet_length - bytes_read) < MAX_PACKET_BYTES);
bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length - bytes_read);
if (dxl_hal_timeout() == 1) {
return false;
}
}
uint16_t expected_crc = UpdateCRC(0, received_data, bytes_read - CHECKSUM_SIZE);
uint16_t received_crc = MakeWord(received_data[bytes_read - 2], received_data[bytes_read - 1]);
if (expected_crc != received_crc) {
// CRC check failed.
return false;
}
*num_bytes_received = bytes_read;
return true;
}
void dxl_rx_packet()
{
unsigned char i, j, nRead;
unsigned char checksum = 0;
if( giBusUsing == 0 )
return;
if( gbInstructionPacket[ID] == BROADCAST_ID )
{
gbCommStatus = COMM_RXSUCCESS;
giBusUsing = 0;
return;
}
if( gbCommStatus == COMM_TXSUCCESS )
{
gbRxGetLength = 0;
gbRxPacketLength = 6;
}
nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
if( gbRxGetLength < gbRxPacketLength )
{
if( dxl_hal_timeout() == 1 )
{
if(gbRxGetLength == 0)
gbCommStatus = COMM_RXTIMEOUT;
else
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
}
// Find packet header
for( i=0; i<(gbRxGetLength-1); i++ )
{
if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff )
{
break;
}
else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff )
{
break;
}
}
if( i > 0 )
{
for( j=0; j<(gbRxGetLength-i); j++ )
gbStatusPacket[j] = gbStatusPacket[j + i];
gbRxGetLength -= i;
}
if( gbRxGetLength < gbRxPacketLength )
{
gbCommStatus = COMM_RXWAITING;
return;
}
// Check id pairing
if( gbInstructionPacket[ID] != gbStatusPacket[ID])
{
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
gbRxPacketLength = gbStatusPacket[LENGTH] + 4;
if( gbRxGetLength < gbRxPacketLength )
{
nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
if( gbRxGetLength < gbRxPacketLength )
{
gbCommStatus = COMM_RXWAITING;
return;
}
}
// Check checksum
for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ )
checksum += gbStatusPacket[i+2];
checksum = ~checksum;
if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum )
{
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
gbCommStatus = COMM_RXSUCCESS;
giBusUsing = 0;
}
void dxl_rx_packet()
{
unsigned char i, j, nRead;
unsigned char checksum = 0;
if( giBusUsing == 0 )
return;
// If Broadcast and not special command like SYNC read, we can bail from here.
if(( gbInstructionPacket[ID] == BROADCAST_ID ) && (gbInstructionPacket[INSTRUCTION] != INST_SYNC_READ ))
{
gbCommStatus = COMM_RXSUCCESS;
giBusUsing = 0;
return;
}
if( gbCommStatus == COMM_TXSUCCESS )
{
gbRxGetLength = 0;
gbRxPacketLength = 6;
// Some device like FTDI benifit from flushing stuff out, others do not.
// Code in flush tries to determine.
dxl_hal_flush(); // make sure everything is writen out
}
nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
if( gbRxGetLength < gbRxPacketLength )
{
if( dxl_hal_timeout() == 1 )
{
if(gbRxGetLength == 0)
gbCommStatus = COMM_RXTIMEOUT;
else
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
}
// Find packet header
for( i=0; i<(gbRxGetLength-1); i++ )
{
if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff )
{
break;
}
else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff )
{
break;
}
}
if( i > 0 )
{
for( j=0; j<(gbRxGetLength-i); j++ )
gbStatusPacket[j] = gbStatusPacket[j + i];
gbRxGetLength -= i;
}
if( gbRxGetLength < gbRxPacketLength )
{
gbCommStatus = COMM_RXWAITING;
return;
}
// Check id pairing
if( gbInstructionPacket[ID] != gbStatusPacket[ID])
{
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
gbRxPacketLength = gbStatusPacket[LENGTH] + 4;
if( gbRxGetLength < gbRxPacketLength )
{
nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
if( gbRxGetLength < gbRxPacketLength )
{
gbCommStatus = COMM_RXWAITING;
return;
}
}
// Check checksum
for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ )
checksum += gbStatusPacket[i+2];
checksum = ~checksum;
if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum )
{
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
gbCommStatus = COMM_RXSUCCESS;
giBusUsing = 0;
}
// receive a status packet
void dxl_rx_packet()
{
unsigned char i, j, nRead;
unsigned char checksum = 0;
// return if bus is busy
if( giBusUsing == 0 )
return;
// if instruction was broadcast there is nothing to wait for
if( gbInstructionPacket[ID] == BROADCAST_ID )
{
gbCommStatus = COMM_RXSUCCESS;
giBusUsing = 0;
return;
}
// check transmission was successful and reset
if( gbCommStatus == COMM_TXSUCCESS )
{
gbRxGetLength = 0;
gbRxPacketLength = 6;
}
// start reading what has been received on the bus
nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
// not finished yet, check for timeout
if( gbRxGetLength < gbRxPacketLength )
{
if( dxl_hal_timeout() == 1 )
{
// timeout, return an error
if(gbRxGetLength == 0)
gbCommStatus = COMM_RXTIMEOUT;
else
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
}
// Find packet header
for( i=0; i<(gbRxGetLength-1); i++ )
{
if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff )
{
break;
}
else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff )
{
break;
}
}
// remove the header and copy to beginning of array
if( i > 0 )
{
for( j=0; j<(gbRxGetLength-i); j++ )
gbStatusPacket[j] = gbStatusPacket[j + i];
gbRxGetLength -= i;
}
// if we haven't received all bytes yet we are still waiting
if( gbRxGetLength < gbRxPacketLength )
{
gbCommStatus = COMM_RXWAITING;
return;
}
// Check id pairing, we only want response from the right Dynamixel
if( gbInstructionPacket[ID] != gbStatusPacket[ID])
{
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
// check the length of the status packet to see if we expect more
gbRxPacketLength = gbStatusPacket[LENGTH] + 4;
if( gbRxGetLength < gbRxPacketLength )
{
// more to come - keep reading
nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
if( gbRxGetLength < gbRxPacketLength )
{
gbCommStatus = COMM_RXWAITING;
return;
}
}
// Check checksum
for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ )
checksum += gbStatusPacket[i+2];
checksum = ~checksum;
if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum )
{
// checksum does not match
gbCommStatus = COMM_RXCORRUPT;
giBusUsing = 0;
return;
}
// everything is fine, return success
gbCommStatus = COMM_RXSUCCESS;
giBusUsing = 0;
}
