diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp index a5a88cef27b34d24313a92eaeac92c6d865b6415..849187d9a64f391ca92d44abee04aff22425c1d1 100644 --- a/src/dynamixel.cpp +++ b/src/dynamixel.cpp @@ -113,7 +113,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned { std::list<std::string> events; unsigned char data_int[256]; - int num=0,read=0,length; + int num=0,read=0,length,start=0; if(this->usb_dev!=NULL) { @@ -137,10 +137,11 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned this->usb_dev->read(&data_int[read],num); read+=num; } - }while(read<4); - length=data_int[3]+4; + this->sync_packet(data_int,read,&start); + }while((read-start)<4); + length=data_int[start+3]+4; // read the remaining of the packet - while(read<length) + while((read-start)<length) { if((num=this->usb_dev->get_num_data())==0) { @@ -159,7 +160,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned } } // check the checksum - if(CDynamixelServer::compute_checksum_v1(data_int,length)!=0x00) + if(CDynamixelServer::compute_checksum_v1(&data_int[start],length)!=0x00) { this->usb_access->exit(); /* handle exceptions */ @@ -170,7 +171,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned if(length>6) { *data=new unsigned char[length-6]; - memcpy(*data,&data_int[5],length-6); + memcpy(*data,&data_int[start+5],length-6); *len=length-6; } else @@ -179,7 +180,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned *len=0; } this->usb_access->exit(); - return data_int[4]; + return data_int[start+4]; }catch(CEventTimeoutException &e){ this->usb_access->exit(); throw e; @@ -337,6 +338,40 @@ void CDynamixel::set_id(unsigned char id) } } +void CDynamixel::sync_packet(unsigned char *data,unsigned int length,int *start) +{ + unsigned int i=0,state=0; + + (*start)=0; + for(i=0;i<length;i++) + { + switch(state) + { + case 0: if(data[i]==0xFF) + state++; + else + (*start)++; + break; + case 1: if(data[i]==0xFF) + state++; + else + { + (*start)--; + state++; + } + break; + case 2: if(data[i]!=0xFF) + { + /* done */ + return; + } + else + (*start)++; + break; + } + } +} + void CDynamixel::resync(void) { std::list<std::string> events; diff --git a/src/dynamixel.h b/src/dynamixel.h index 4bb71034ad89818befc33fb130c03b761910ee3b..6f8ca0b38ba7754713fa6b49940952902c58c834 100644 --- a/src/dynamixel.h +++ b/src/dynamixel.h @@ -77,6 +77,11 @@ class CDynamixel * */ void set_baudrate(int baudrate); + /** + * \brief + * + */ + void sync_packet(unsigned char *data,unsigned int length,int *start); /** * \brief *