Skip to content
Snippets Groups Projects
Commit d414f570 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Used the same solution to optimize the operation of the dynamixel master.

parent 8e2f7be8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_base/include/dynamixel_master.h
+ 1
− 21
View file @ d414f570
......@@ -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
*
......
dynamixel_base/src/dynamixel_master.c
+ 58
− 106
View file @ d414f570
......@@ -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();
......
dynamixel_base/src/dynamixel_slave.c
+ 0
− 15
View file @ d414f570
......@@ -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;
......
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