Added a function to synchronize the input data with the packet header in case...

Added a function to synchronize the input data with the packet header in case unexpected data is received.

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;

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
      *