diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 849187d9a64f391ca92d44abee04aff22425c1d1..bc82acfc918127c9142dc2a2f99bc7c7769ece55 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -112,7 +112,7 @@ void CDynamixel::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,
unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned char *len)
{
std::list<std::string> events;
- unsigned char data_int[256];
+ unsigned char data_int[1024];
int num=0,read=0,length,start=0;
if(this->usb_dev!=NULL)
@@ -127,17 +127,17 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
this->event_server->wait_all(events,20);
num=this->usb_dev->get_num_data();
}
- if((read+num)>256)
+ if((read+num)>1024)
{
- this->usb_dev->read(&data_int[read],256-read);
- read=256;
+ this->usb_dev->read(&data_int[read],1024-read);
+ read=1024;
}
else
{
this->usb_dev->read(&data_int[read],num);
read+=num;
}
- this->sync_packet(data_int,read,&start);
+ this->sync_packet_v1(data_int,read,&start);
}while((read-start)<4);
length=data_int[start+3]+4;
// read the remaining of the packet
@@ -148,10 +148,10 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
this->event_server->wait_all(events,20);
num=this->usb_dev->get_num_data();
}
- if((read+num)>256)
+ if((read-start+num)>length)
{
- this->usb_dev->read(&data_int[read],256-read);
- read=256;
+ this->usb_dev->read(&data_int[read],length-read);
+ read=length;
}
else
{
@@ -197,7 +197,7 @@ unsigned char CDynamixel::receive_status_packet_v2(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;
unsigned short int crc;
if(this->usb_dev!=NULL)
@@ -222,17 +222,18 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
this->usb_dev->read(&data_int[read],num);
read+=num;
}
- }while(read<7);
- length=data_int[5]+data_int[6]*256+7;
+ this->sync_packet_v2(data_int,read,&start);
+ }while((read-start)<7);
+ length=data_int[start+5]+data_int[start+6]*256+7;
// read the remaining of the packet
- while(read<length)
+ while((read-start)<length)
{
if((num=this->usb_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
num=this->usb_dev->get_num_data();
}
- if((read+num)>256)
+ if((read-start+num)>256)
{
this->usb_dev->read(&data_int[read],256-read);
read=256;
@@ -244,8 +245,8 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
}
}
// check the checksum
- crc=CDynamixelServer::compute_checksum_v2(data_int,length-2);
- if((crc%256)!=data_int[length-2] || (crc/256)!=data_int[length-1])
+ crc=CDynamixelServer::compute_checksum_v2(&data_int[start],length-2);
+ if((crc%256)!=data_int[start+length-2] || (crc/256)!=data_int[start+length-1])
{
this->usb_access->exit();
/* handle exceptions */
@@ -255,7 +256,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
if(length>11)
{
*data=new unsigned char[length-11];
- memcpy(*data,&data_int[9],length-11);
+ memcpy(*data,&data_int[start+9],length-11);
*len=length-11;
}
else
@@ -264,7 +265,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
*len=0;
}
this->usb_access->exit();
- return data_int[8];
+ return data_int[start+8];
}catch(CEventTimeoutException &e){
this->usb_access->exit();
throw e;
@@ -338,12 +339,11 @@ void CDynamixel::set_id(unsigned char id)
}
}
-void CDynamixel::sync_packet(unsigned char *data,unsigned int length,int *start)
+void CDynamixel::sync_packet_v1(unsigned char *data,unsigned int length,int *start)
{
- unsigned int i=0,state=0;
+ unsigned int i=0,state=0,start_len=(*start);
- (*start)=0;
- for(i=0;i<length;i++)
+ for(i=start_len;i<length;i++)
{
switch(state)
{
@@ -356,8 +356,8 @@ void CDynamixel::sync_packet(unsigned char *data,unsigned int length,int *start)
state++;
else
{
- (*start)--;
- state++;
+ (*start)++;
+ state--;
}
break;
case 2: if(data[i]!=0xFF)
@@ -372,6 +372,64 @@ void CDynamixel::sync_packet(unsigned char *data,unsigned int length,int *start)
}
}
+void CDynamixel::sync_packet_v2(unsigned char *data,unsigned int length,int *start)
+{
+ unsigned int i=0,state=0,start_len=(*start);
+
+ for(i=start_len;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]==0xFD)
+ state++;
+ else
+ {
+ if(data[i]==0xFF)
+ {
+ state--;
+ (*start)++;
+ }
+ else
+ {
+ state=0;
+ (*start)+=2;
+ }
+ }
+ break;
+ case 3: if(data[i]==0x00)
+ return;
+ else
+ {
+ if(data[i]==0xFF)
+ {
+ state=1;
+ (*start)+=2;
+ }
+ else
+ {
+ state=0;
+ (*start)+=3;
+ }
+ }
+ break;
+ }
+ }
+}
+
+
void CDynamixel::resync(void)
{
std::list<std::string> events;
diff --git a/src/dynamixel.h b/src/dynamixel.h
index 6f8ca0b38ba7754713fa6b49940952902c58c834..6bc72295e4010c19d241b24cb68dd50797151eaa 100644
--- a/src/dynamixel.h
+++ b/src/dynamixel.h
@@ -81,7 +81,12 @@ class CDynamixel
* \brief
*
*/
- void sync_packet(unsigned char *data,unsigned int length,int *start);
+ void sync_packet_v1(unsigned char *data,unsigned int length,int *start);
+ /**
+ * \brief
+ *
+ */
+ void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
/**
* \brief
*
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index f7ba1a01b2a45e3eb9f130171720e2427f4d3d16..272bf31e269a273a27d802df5afb2aac96d60340 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -203,8 +203,8 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,unsigned char *len)
{
std::list<std::string> events;
- unsigned char data_int[256];
- int num=0,read=0,length;
+ unsigned char data_int[1024];
+ int num=0,read=0,length,start=0;
if(this->comm_dev!=NULL)
{
@@ -218,27 +218,28 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
this->event_server->wait_all(events,20);
num=this->comm_dev->get_num_data();
}
- if((read+num)>4)
+ if((read+num)>1024)
{
- this->comm_dev->read(&data_int[read],4-read);
- read=4;
+ this->comm_dev->read(&data_int[read],1024-read);
+ read=1024;
}
else
{
this->comm_dev->read(&data_int[read],num);
read+=num;
}
- }while(read<4);
- length=data_int[3]+4;
+ this->sync_packet_v1(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->comm_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
num=this->comm_dev->get_num_data();
}
- if((read+num)>length)
+ if((read-start+num)>length)
{
this->comm_dev->read(&data_int[read],length-read);
read=length;
@@ -250,7 +251,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
}
}
// check the checksum
- if(this->compute_checksum_v1(data_int,length)!=0x00)
+ if(this->compute_checksum_v1(&data_int[start],length)!=0x00)
{
this->dynamixel_access.exit();
/* handle exceptions */
@@ -261,7 +262,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
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
@@ -270,7 +271,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
*len=0;
}
this->dynamixel_access.exit();
- return data_int[4];
+ return data_int[start+4];
}catch(CEventTimeoutException &e){
this->dynamixel_access.exit();
throw e;
@@ -286,8 +287,8 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
{
std::list<std::string> events;
- unsigned char data_int[256];
- int num=0,read=0,length;
+ unsigned char data_int[1024];
+ int num=0,read=0,length,start=0;
unsigned short int crc;
if(this->comm_dev!=NULL)
@@ -302,27 +303,28 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
this->event_server->wait_all(events,20);
num=this->comm_dev->get_num_data();
}
- if((read+num)>7)
+ if((read+num)>1024)
{
- this->comm_dev->read(&data_int[read],7-read);
- read=7;
+ this->comm_dev->read(&data_int[read],1024-read);
+ read=1024;
}
else
{
this->comm_dev->read(&data_int[read],num);
read+=num;
}
+ this->sync_packet_v2(data_int,read,&start);
}while(read<7);
- length=data_int[5]+data_int[6]*256+7;
+ length=data_int[start+5]+data_int[start+6]*256+7;
// read the remaining of the packet
- while(read<length)
+ while((read-start)<length)
{
if((num=this->comm_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
num=this->comm_dev->get_num_data();
}
- if((read+num)>length)
+ if((read-start+num)>length)
{
this->comm_dev->read(&data_int[read],length-read);
read=length;
@@ -334,8 +336,8 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
}
}
// check the checksum
- crc=this->compute_checksum_v2(data_int,length-2);
- if((crc%256)!=data_int[length-2] || (crc/256)!=data_int[length-1])
+ crc=this->compute_checksum_v2(&data_int[start],length-2);
+ if((crc%256)!=data_int[start+length-2] || (crc/256)!=data_int[start+length-1])
{
this->dynamixel_access.exit();
/* handle exceptions */
@@ -345,7 +347,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
if(length>11)
{
*data=new unsigned char[length-11];
- memcpy(*data,&data_int[9],length-11);
+ memcpy(*data,&data_int[start+9],length-11);
*len=length-11;
}
else
@@ -354,7 +356,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
*len=0;
}
this->dynamixel_access.exit();
- return data_int[8];
+ return data_int[start+8];
}catch(CEventTimeoutException &e){
this->dynamixel_access.exit();
throw e;
@@ -501,6 +503,96 @@ void CDynamixelServer::handle_error(unsigned char error)
}
}
+void CDynamixelServer::sync_packet_v1(unsigned char *data,unsigned int length,int *start)
+{
+ unsigned int i=0,state=0,start_len=(*start);
+
+ for(i=start_len;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 CDynamixelServer::sync_packet_v2(unsigned char *data,unsigned int length,int *start)
+{
+ unsigned int i=0,state=0,start_len=(*start);
+
+ for(i=start_len;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]==0xFD)
+ state++;
+ else
+ {
+ if(data[i]==0xFF)
+ {
+ state--;
+ (*start)++;
+ }
+ else
+ {
+ state=0;
+ (*start)+=2;
+ }
+ }
+ break;
+ case 3: if(data[i]==0x00)
+ return;
+ else
+ {
+ if(data[i]==0xFF)
+ {
+ state=1;
+ (*start)+=2;
+ }
+ else
+ {
+ state=0;
+ (*start)+=3;
+ }
+ }
+ break;
+ }
+ }
+}
+
void *CDynamixelServer::scan_thread(void *param)
{
CDynamixelServer *dyn_server=(CDynamixelServer *)param;
diff --git a/src/dynamixelserver.h b/src/dynamixelserver.h
index f941278d33ece74ec8aff68edd7e797ddbdabd2d..763e3fc56f50ea5636e7adb4de1fbc31eedfbba5 100644
--- a/src/dynamixelserver.h
+++ b/src/dynamixelserver.h
@@ -165,6 +165,16 @@ class CDynamixelServer
*
*/
unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+ /**
+ * \brief
+ *
+ */
+ void sync_packet_v1(unsigned char *data,unsigned int length,int *start);
+ /**
+ * \brief
+ *
+ */
+ void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
protected:
/**
* \brief