From 042183c31d57807fecce6af795e8335a2a3434e6 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Sep 2020 13:00:21 +0200
Subject: [PATCH] Improved the dyn_master_relay function with internal protocol
version conversion.
---
dynamixel_base/include/dynamixel_master.h | 2 +-
dynamixel_base/src/dynamixel_master.c | 34 ++++++++++++++++++++---
2 files changed, 31 insertions(+), 5 deletions(-)
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 70fd70d..64df2a8 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/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
}
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index a91820b..0fb6a96 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/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)
{
- dyn2_copy_packet(inst_pkt,master->tx_buffer);
+ 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,7 +1335,20 @@ 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);
- dyn2_copy_packet(master->rx_buffer,status_pkt);
+ 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;
--
GitLab