From 817eaecb6611337612f0dbf1316ba6df9787120c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 19 Oct 2015 16:43:12 +0000
Subject: [PATCH] Solved aproblem in the sync_Read and sync_write commands when
 one of the addressed devices does not respond in time.

---
 dynamixel_base/include/dynamixel.h    | 1 +
 dynamixel_base/include/dynamixel2.h   | 1 +
 dynamixel_base/src/dynamixel.c        | 6 ++++++
 dynamixel_base/src/dynamixel2.c       | 5 +++++
 dynamixel_base/src/dynamixel_master.c | 2 ++
 5 files changed, 15 insertions(+)

diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h
index 8b73f9c..263e03d 100755
--- a/dynamixel_base/include/dynamixel.h
+++ b/dynamixel_base/include/dynamixel.h
@@ -72,5 +72,6 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err
 inline TDynError dyn_get_status_error(unsigned char *packet);
 /* read instruction status packet */
 unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data);
+inline unsigned char dyn_get_read_status_id(unsigned char *packet);
 
 #endif
diff --git a/dynamixel_base/include/dynamixel2.h b/dynamixel_base/include/dynamixel2.h
index f51473c..a2d8e30 100644
--- a/dynamixel_base/include/dynamixel2.h
+++ b/dynamixel_base/include/dynamixel2.h
@@ -80,5 +80,6 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er
 inline TDynError dyn2_get_status_error(unsigned char *packet);
 /* read instruction status packet */
 unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char *data);
+inline unsigned char dyn2_get_read_status_id(unsigned char *packet);
 
 #endif
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index 8a72020..659c3cd 100755
--- a/dynamixel_base/src/dynamixel.c
+++ b/dynamixel_base/src/dynamixel.c
@@ -330,3 +330,9 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data
 
   return packet[DYN_LENGTH_OFF]-0x02;
 }
+
+inline unsigned char dyn_get_read_status_id(unsigned char *packet)
+{
+  return packet[DYN_ID_OFF];
+}
+
diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c
index b85db07..85c6cec 100644
--- a/dynamixel_base/src/dynamixel2.c
+++ b/dynamixel_base/src/dynamixel2.c
@@ -483,3 +483,8 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char
 
   return length;
 }
+
+inline unsigned char dyn2_get_read_status_id(unsigned char *packet)
+{
+  return packet[DYN2_ID_OFF];
+}
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 292a25f..62e202c 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -519,6 +519,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
       {
         if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
         {
+          while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
           if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length)// not enough data
             error=DYN_INST_ERROR;
         }
@@ -578,6 +579,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
         }
         else
         {
+          while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
           if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
             error=DYN_INST_ERROR;
         }
-- 
GitLab