Solved a bug in the slave input state machine to process the incomming data.

src/dynamixel_slave.cpp

@@ -88,9 +88,9 @@ void *CDynamixelSlave::process_packets_thread(void *params)
                           else
                             slave->num_bytes--;
                           break;
-                  case 2: if(data[i]==0xFD)// version 2 header
+                  case 2: if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
                           {
-                            if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
+                            if(data[i]==0xFD)// version 2 header
                             {
                               slave->packet[slave->num_bytes]=data[i];
                               slave->num_bytes++;
@@ -98,10 +98,13 @@ void *CDynamixelSlave::process_packets_thread(void *params)
                             else
                               slave->num_bytes=0;// ignore packet and restart synchronization
                           }
-                          else if(data[i]!=0xFF)
+                          else 
                           {
-                            slave->packet[slave->num_bytes]=data[i];
-                            slave->num_bytes++;
+                            if(data[i]!=0xFF)
+                            {
+                              slave->packet[slave->num_bytes]=data[i];
+                              slave->num_bytes++;
+                            }
                           }
                           break;
                   case 3: slave->packet[slave->num_bytes]=data[i];

src/dynamixelserver.cpp

@@ -467,9 +467,9 @@ void CDynamixelServer::receive_sync_bulk_status_packets_v2(std::vector<unsigned
           }
           else
             need_data=true;
-         }
-         if(length!=-1 && num>=length)
-         {
+        }
+        if(length!=-1 && num>=length)
+        {
           // check the checksum
           crc=this->compute_checksum_v2(&data_int[read],length-2);
           if((crc%256)!=data_int[read+length-2] || (crc/256)!=data_int[read+length-1])