Improved the dyn_master_relay function with internal protocol version conversion.

dynamixel_base/include/dynamixel_master.h

@@ -214,7 +214,7 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
  * \brief
  *
  */
-unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt);
+unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt);
 
 #ifdef __cplusplus
 }

dynamixel_base/src/dynamixel_master.c

@@ -1297,13 +1297,26 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
   return error;
 }
 
-unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt)
+unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt)
 {
-  unsigned char error=DYN_COMM_ERROR;
+  unsigned char error=DYN_COMM_ERROR,i;
 
   if(master!=0x00000000)
   {
+    if(inst_ver==DYN_VER1)
+    {
+      if(master->version==DYN_VER1)
+        dyn_copy_packet(inst_pkt,master->tx_buffer);
+      else
+        dyn2_convert_v1_inst_packet(inst_pkt,master->tx_buffer);
+    }
+    else
+    {
+      if(master->version==DYN_VER1)
+        dyn_convert_v2_inst_packet(inst_pkt,master->tx_buffer);
+      else
         dyn2_copy_packet(inst_pkt,master->tx_buffer);
+    }
     master->rx_num_packets=0x01;
     master->rx_no_answer=0x00;
     // enable transmission
@@ -1322,8 +1335,21 @@ unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,
     }
     // wait for the replay within the given timeout
     error=dyn_master_wait_reception(master);
+    if(inst_ver==DYN_VER1)
+    {
+      if(master->version==DYN_VER1)
+        dyn_copy_packet(master->rx_buffer,status_pkt);
+      else
+        dyn_convert_v2_status_packet(master->rx_buffer,status_pkt);
+    }
+    else
+    {
+      if(master->version==DYN_VER1)
+        dyn2_convert_v1_status_packet((TDynInstruction)inst_pkt[DYN2_INST_OFF],master->rx_buffer,status_pkt);
+      else
         dyn2_copy_packet(master->rx_buffer,status_pkt);
     }
+  }
 
   return error;
 }