From d414f57003d04136adf8d5c616003141034f4dc6 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sat, 8 Jun 2024 13:18:52 +0200
Subject: [PATCH] Used the same solution to optimize the operation of the
dynamixel master.
---
dynamixel_base/include/dynamixel_master.h | 22 +--
dynamixel_base/src/dynamixel_master.c | 164 ++++++++--------------
dynamixel_base/src/dynamixel_slave.c | 15 --
3 files changed, 59 insertions(+), 142 deletions(-)
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 740162c..8edda63 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -58,21 +58,11 @@ typedef struct
*
*/
unsigned char packet_ready;
- /**
- * \brief
- *
- */
- unsigned char rx_dma_mode;
/**
* \brief
*
*/
volatile unsigned char tx_done;
- /**
- * \brief
- *
- */
- unsigned short int sync_bulk_address;
/**
* \brief
*
@@ -112,17 +102,7 @@ typedef struct
* \brief
*
*/
- unsigned char (*tx_irq)(void *hal_dev, const unsigned char *data, unsigned short int length);
- /**
- * \brief
- *
- */
- unsigned char (*rx_irq)(void *hal_dev, const unsigned char *data, unsigned short int length);
- /**
- * \brief
- *
- */
- unsigned char (*abort_irq)(void *hal_dev);
+ unsigned short int rx_length;
/**
* \brief
*
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 3dbd2b6..0e7ead5 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -3,64 +3,8 @@
/* private functions */
void dyn_master_receive_cb(TDynamixelMaster *master)
{
- unsigned short int length;
-
if(master!=0x00000000)
- {
- if(master->rx_dma_mode==0x01)
- {
- master->rx_dma_mode=0x00;
- master->rx_num_packets--;
- if(master->rx_num_packets==0x00)
- {
- time_cancel_timeout(&master->time);
- master->sync_bulk_address=0x0000;
- master->packet_ready=0x01;
- }
- else
- {
- if(master->version==DYN_VER1)
- {
- if(master->rx_num_packets==0x01)
- master->sync_bulk_address=0x0000;
- master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],4);
- }
- else
- {
- if(master->rx_num_packets==0x01)
- master->sync_bulk_address=0x0000;
- master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],7);
- }
- }
- }
- else
- {
- master->rx_dma_mode=0x01;
- if(master->version==DYN_VER1)
- {
- if(master->rx_buffer[0]!=0xFF) return;
- if(master->rx_buffer[1]!=0xFF) return;
- if(master->hal_dev!=0x00000000)
- {
- length=master->rx_buffer[master->sync_bulk_address+3];
- master->rx_dma(master->hal_dev,&master->rx_buffer[master->sync_bulk_address+4],length);
- master->sync_bulk_address+=length+4;
- }
- }
- else
- {
- if(master->rx_buffer[0]!=0xFF) return;
- if(master->rx_buffer[1]!=0xFF) return;
- if(master->rx_buffer[2]!=0xFD) return;
- if(master->hal_dev!=0x00000000)
- {
- length=master->rx_buffer[master->sync_bulk_address+5]+(master->rx_buffer[master->sync_bulk_address+6]<<8);
- master->rx_dma(master->hal_dev,&master->rx_buffer[master->sync_bulk_address+7],length);
- master->sync_bulk_address+=length+7;
- }
- }
- }
- }
+ master->packet_ready=0x01;
}
void dyn_master_send_cb(TDynamixelMaster *master)
@@ -71,14 +15,7 @@ void dyn_master_send_cb(TDynamixelMaster *master)
if(master->rx_no_answer)
master->rx_no_answer=0x00;
else
- {
- if(master->version==DYN_VER1)
- master->rx_irq(master->hal_dev,master->rx_buffer,4);
- else
- {
- master->rx_irq(master->hal_dev,master->rx_buffer,7);
- }
- }
+ master->rx_dma(master->hal_dev,master->rx_buffer,master->rx_length);
master->tx_done=0x01;
}
}
@@ -103,16 +40,6 @@ void dummy_dyn_master_disable_power(void)
}
-unsigned char dummy_dyn_master_tx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
-{
- return DYN_SUCCESS;
-}
-
-unsigned char dummy_dyn_master_rx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
-{
- return DYN_SUCCESS;
-}
-
unsigned char dummy_dyn_master_tx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
@@ -123,11 +50,6 @@ unsigned char dummy_dyn_master_rx_dma(void *hal_dev, const unsigned char *data,
return DYN_SUCCESS;
}
-unsigned char dummy_dyn_master_abort_irq(void *hal_dev)
-{
- return DYN_SUCCESS;
-}
-
unsigned char dummy_dyn_master_abort_dma(void *hal_dev)
{
return DYN_SUCCESS;
@@ -200,7 +122,6 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
if(time_is_timeout(&master->time))
{
/* cancel any IRQ or DMA reception */
- master->abort_irq(master->hal_dev);
master->abort_dma(master->hal_dev);
return DYN_TIMEOUT;
}
@@ -221,7 +142,6 @@ unsigned char dyn_master_is_reception_done(TDynamixelMaster *master)
if(time_is_timeout(&master->time))
{
/* cancel any IRQ or DMA reception */
- master->abort_irq(master->hal_dev);
master->abort_dma(master->hal_dev);
return DYN_TIMEOUT;
}
@@ -246,10 +166,15 @@ unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char
{
// generate the read packet for the desired device
if(master->version==DYN_VER1)
+ {
dyn_init_read_packet(master->tx_buffer,id,address,length);
+ master->rx_length=length+4;
+ }
else
+ {
dyn2_init_read_packet(master->tx_buffer,id,address,length);
- master->rx_num_packets=0x01;
+ master->rx_length=length+7;
+ }
if(master->return_level==no_return || id==DYN_BROADCAST_ID)
master->rx_no_answer=0x01;
else
@@ -342,10 +267,15 @@ unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned cha
{
// generate the read packet for the desired device
if(master->version==DYN_VER1)
+ {
dyn_init_write_packet(master->tx_buffer,id,address,length,data);
+ master->rx_length=6;
+ }
else
+ {
dyn2_init_write_packet(master->tx_buffer,id,address,length,data);
- master->rx_num_packets=0x01;
+ master->rx_length=11;
+ }
if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
master->rx_no_answer=0x00;
else
@@ -421,8 +351,7 @@ unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char
dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
else
dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
- master->rx_num_packets=0x01;
- master->rx_no_answer=0x00;
+ master->rx_no_answer=0x01;
// enable transmission
master->set_tx_mode();
// send the data
@@ -458,7 +387,7 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
if(master->version==DYN_VER2)
{
dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length);
- master->rx_num_packets=num;
+ master->rx_length=(length+7)*num;
if(master->return_level==no_return)
master->rx_no_answer=0x01;
else
@@ -545,8 +474,8 @@ unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char
if(master->version==DYN_VER2)
{
dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data);
- master->rx_num_packets=0x01;
- master->rx_no_answer=0x00;
+ master->rx_length=0;
+ master->rx_no_answer=0x01;
// enable transmission
master->set_tx_mode();
// send the data
@@ -586,16 +515,22 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
// generate the read packet for the desired device
if(master->version==DYN_VER1)
{
+ master->rx_length=0;
for(i=0;i<num;i++)
{
ver1_address[i]=address[i];
ver1_length[i]=length[i];
+ master->rx_length=(length[i]+4);
}
dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length);
}
else
+ {
dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
- master->rx_num_packets=num;
+ master->rx_length=0;
+ for(i=0;i<num;i++)
+ master->rx_length=(length[i]+7);
+ }
if(master->return_level==no_return)
master->rx_no_answer=0x01;
else
@@ -688,16 +623,12 @@ void dyn_master_init(TDynamixelMaster *master,void *hal_dev,TDynVersion version)
{
master->hal_dev=hal_dev;
master->version=version;
- master->rx_dma_mode=0x00;
master->tx_done=0x01;
/* initialize the internal callbacks */
master->set_tx_mode=dummy_dyn_master_set_tx_mode;
master->set_rx_mode=dummy_dyn_master_set_rx_mode;
master->enable_power=dummy_dyn_master_enable_power;
master->disable_power=dummy_dyn_master_disable_power;
- master->tx_irq=dummy_dyn_master_tx_irq;
- master->rx_irq=dummy_dyn_master_rx_irq;
- master->abort_irq=dummy_dyn_master_abort_irq;
master->tx_dma=dummy_dyn_master_tx_dma;
master->rx_dma=dummy_dyn_master_rx_dma;
master->abort_dma=dummy_dyn_master_abort_dma;
@@ -705,9 +636,8 @@ void dyn_master_init(TDynamixelMaster *master,void *hal_dev,TDynVersion version)
master->packet_ready=0x00;
master->rx_timeout_ms=50;
master->rx_no_answer=0x00;
- master->rx_num_packets=0x00;
+ master->rx_length=0x0000;
master->return_level=return_all;
- master->sync_bulk_address=0x0000;
master->set_rx_mode();
}
@@ -738,10 +668,15 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
{
// generate the ping packet for the desired device
if(master->version==DYN_VER1)
+ {
dyn_init_ping_packet(master->tx_buffer,id);
+ master->rx_length=6;
+ }
else
+ {
dyn2_init_ping_packet(master->tx_buffer,id);
- master->rx_num_packets=0x01;
+ master->rx_length=14;
+ }
master->rx_no_answer=0x00;
// enable transmission
master->set_tx_mode();
@@ -805,10 +740,15 @@ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,un
{
// generate the read packet for the desired device
if(master->version==DYN_VER1)
+ {
dyn_init_read_packet(master->tx_buffer,id,address,length);
+ master->rx_length=length+4;
+ }
else
+ {
dyn2_init_read_packet(master->tx_buffer,id,address,length);
- master->rx_num_packets=0x01;
+ master->rx_length=length+7;
+ }
if(master->return_level==no_return || id==DYN_BROADCAST_ID)
master->rx_no_answer=0x01;
else
@@ -893,10 +833,15 @@ unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id,
{
// generate the write packet for the desired device
if(master->version==DYN_VER1)
+ {
dyn_init_write_packet(master->tx_buffer,id,address,length,data);
+ master->rx_length=6;
+ }
else
+ {
dyn2_init_write_packet(master->tx_buffer,id,address,length,data);
- master->rx_num_packets=0x01;
+ master->rx_length=11;
+ }
if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
master->rx_no_answer=0x00;
else
@@ -950,10 +895,15 @@ unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, un
{
// generate the registered write packet for the desired device
if(master->version==DYN_VER1)
+ {
dyn_init_reg_write_packet(master->tx_buffer,id,address,length,data);
+ master->rx_length=6;
+ }
else
+ {
dyn2_init_reg_write_packet(master->tx_buffer,id,address,length,data);
- master->rx_num_packets=0x01;
+ master->rx_length=11;
+ }
if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
master->rx_no_answer=0x00;
else
@@ -1010,7 +960,6 @@ unsigned char dyn_master_action(TDynamixelMaster *master)
dyn_init_action_packet(master->tx_buffer);
else
dyn2_init_action_packet(master->tx_buffer);
- master->rx_num_packets=0x01;
master->rx_no_answer=0x01;
// enable transmission
master->set_tx_mode();
@@ -1042,7 +991,6 @@ unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,u
dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
else
dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
- master->rx_num_packets=0x01;
master->rx_no_answer=0x01;
// enable transmission
master->set_tx_mode();
@@ -1073,7 +1021,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
if(master->version==DYN_VER2)
{
dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length);
- master->rx_num_packets=num;
+ master->rx_length=(length+7)*num;
if(master->return_level==no_return)
master->rx_no_answer=0x01;
else
@@ -1130,16 +1078,22 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
// generate the read packet for the desired device
if(master->version==DYN_VER1)
{
+ master->rx_length=0;
for(i=0;i<num;i++)
{
ver1_address[i]=address[i];
ver1_length[i]=length[i];
+ master->rx_length=length[i]+4;
}
dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length);
}
else
+ {
dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
- master->rx_num_packets=num;
+ master->rx_length=0;
+ for(i=0;i<num;i++)
+ master->rx_length=(length[i]+7);
+ }
if(master->return_level==no_return)
master->rx_no_answer=0x01;
else
@@ -1216,7 +1170,6 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
if(master->version==DYN_VER2)
{
dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data);
- master->rx_num_packets=0x01;
master->rx_no_answer=0x01;
// enable transmission
master->set_tx_mode();
@@ -1258,7 +1211,6 @@ unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,uns
else
dyn2_copy_packet(inst_pkt,master->tx_buffer);
}
- master->rx_num_packets=0x01;
master->rx_no_answer=0x00;
// enable transmission
master->set_tx_mode();
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index c48e500..c284434 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -31,16 +31,6 @@ unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_p
return DYN_NO_DEVICE;
}
-unsigned char dummy_dyn_slave_tx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
-{
- return DYN_SUCCESS;
-}
-
-unsigned char dummy_dyn_slave_rx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
-{
- return DYN_SUCCESS;
-}
-
unsigned char dummy_dyn_slave_tx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
@@ -51,11 +41,6 @@ unsigned char dummy_dyn_slave_rx_dma(void *hal_dev, const unsigned char *data, u
return DYN_SUCCESS;
}
-unsigned char dummy_dyn_slave_abort_irq(void *hal_dev)
-{
- return DYN_SUCCESS;
-}
-
unsigned char dummy_dyn_slave_abort_dma(void *hal_dev)
{
return DYN_SUCCESS;
--
GitLab