diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h index 8b73f9c5db022f826b4715432de4ab606dd7923e..263e03d87a06c2a24572d46605b96be38eb7da85 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 f51473c7c62e342bd9f12875f184f165e9944f5b..a2d8e30392d74b6429393f8a3d280532fbd99755 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 8a72020d537468250434b8229995895bd09335ef..659c3cdbd93c7c5ac5f4e8a8791c58adb4d58c7e 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 b85db07a219a7e99d4a6e17114b40f7b73c6f865..85c6cec685ceffbf02dc8bb061bb0d21d08a4065 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 292a25f12f597d26c9f19780e82bd536c7ed64e7..62e202cc3f14488f04f8196f624fe335b40236f6 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; }