Commit 042183c3 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Improved the dyn_master_relay function with internal protocol version conversion.

parent 9b2d34e1
......@@ -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
}
......
......@@ -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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment