Solved a bug in the reception timeout: It was continually updated so when...

Solved a bug in the reception timeout: It was continually updated so when there was an error, the main loop got stuck.

dynamixel_base/src/dynamixel_master.c

@@ -247,21 +247,6 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
     }
     master->packet_ready=0x00;
     return DYN_SUCCESS;
-    // check the input packet checksum
-    if(master->version==DYN_VER1)
-    {
-      if(dyn_check_checksum(master->rx_buffer)==0xFF)
-        return dyn_get_status_error(master->rx_buffer);
-      else
-        return DYN_CHECKSUM_ERROR;
-    }
-    else
-    {
-      if(dyn2_check_checksum(master->rx_buffer)==0x01)
-        return dyn2_get_status_error(master->rx_buffer);
-      else
-        return DYN_CHECKSUM_ERROR;
-    }
   }
   else
     return DYN_COMM_ERROR;
@@ -326,17 +311,20 @@ unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char
 
 unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
 {
+  static unsigned char tx_done=0;
   unsigned char error=DYN_COMM_ERROR;
 
   if(master!=0x00000000)
   {
-    if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+    if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
     {
       // wait for the replay within the given timeout
       if(master->return_level!=no_return && id!=DYN_BROADCAST_ID)
       {
+        tx_done=1;
         if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
         {
+          tx_done=0;
           // check the input packet checksum
           if(master->version==DYN_VER1)
           {
@@ -373,6 +361,11 @@ unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned ch
               return DYN_CHECKSUM_ERROR;
           }
         }
+        else if(error==DYN_TIMEOUT)
+        {
+          tx_done=0;
+          return error;
+        }
         else
           return error;
       }
@@ -414,17 +407,20 @@ unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned cha
 
 unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
 {
+  static unsigned char tx_done=0;
   unsigned char error=DYN_COMM_ERROR;
 
   if(master!=0x00000000)
   {
-    if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+    if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
     {
       // wait for the replay within the given timeout
       if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
       {
+        tx_done=1;
         if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
         {
+          tx_done=0;
           // check the input packet checksum
           if(master->version==DYN_VER1)
           {
@@ -447,6 +443,11 @@ unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned c
       else
         return DYN_SUCCESS;
     }
+    else if(error==DYN_TIMEOUT) 
+    {
+      tx_done=0;
+      return error;
+    }
     else
       return error;
   }
@@ -530,6 +531,7 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
 
 unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
 {
+  static unsigned char tx_done=0;
   unsigned char error=DYN_COMM_ERROR,i;
 
   if(master!=0x00000000)
@@ -537,13 +539,15 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha
     // generate the read packet for the desired device
     if(master->version==DYN_VER2)
     {
-      if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+      if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
       {
         // wait for the replay within the given timeout
         if(master->return_level!=no_return)
         {
+          tx_done=1;
           if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
           {
+            tx_done=0;
             for(i=0;i<num;i++)
             {
               if(dyn2_check_checksum(&master->rx_buffer[(length+11)*i])==0x01)
@@ -567,6 +571,11 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha
         else
           return DYN_SUCCESS;
       }
+      else if(error==DYN_TIMEOUT)
+      {
+        tx_done=0;
+        return error;
+      }
       else
         return error;
     }
@@ -666,17 +675,20 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
 unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
 {
   unsigned char error=DYN_COMM_ERROR,i;
+  static unsigned char tx_done=0;
   unsigned short int start=0;
 
   if(master!=0x00000000)
   {
-    if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+    if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
     {
       // wait for the replay within the given timeout
       if(master->return_level!=no_return)
       {
+        tx_done=1;
         if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
         {
+          tx_done=0;
           for(i=0;i<num;i++)
           {
             // check the input packet checksum
@@ -715,6 +727,11 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
           }
           return DYN_SUCCESS;
         }
+        else if(error==DYN_TIMEOUT)
+        {
+          tx_done=0;
+          return error;
+        }
         else
           return error;
       }