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