When starting a new transmission, it does not wait for the previous one to...

When starting a new transmission, it does not wait for the previous one to finish (it must alrady have finished).

dynamixel_base/src/dynamixel_slave.c

@@ -5,8 +5,6 @@ unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char
 {
   if(dyn_slave!=0x00000000)
   {
-    if(dyn_slave->comm_dev->time!=0x00000000)
-      time_set_timeout(dyn_slave->comm_dev->time,dyn_slave->rx_timeout_ms*1000);
     switch(dyn_slave->received_bytes)
     {
       case 0: if(byte==0xFF)
@@ -14,6 +12,8 @@ unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char
                 dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
                 dyn_slave->received_bytes++;
               }
+              if(dyn_slave->comm_dev->time!=0x00000000)
+                time_set_timeout(dyn_slave->comm_dev->time,dyn_slave->rx_timeout_ms*1000);
               break;
       case 1: if(byte==0xFF)
               {
@@ -123,8 +123,8 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
 {
   if(slave!=0x00000000)
   {
-    // wait until the previous transmission has ended (if any)
-    while(comm_is_send_done(slave->comm_dev)==COMM_BUSY);
+    // cancel any transmission pending 
+    comm_do_dma_send(slave->comm_dev);
     if(return_delay>0)
       if(slave->comm_dev->time!=0x00000000)
         time_delay_us(slave->comm_dev->time,return_delay<<1);