diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h index 203be76a60d8da9fbc33d06282090f8fe68caaf9..d19c55cebca0f7822327cb58d8423a4188fc6710 100644 --- a/dynamixel_base/include/dynamixel_slave.h +++ b/dynamixel_base/include/dynamixel_slave.h @@ -50,11 +50,6 @@ typedef struct TDynamixelSlave * */ TDynVersion version; - /** - * \brief - * - */ - unsigned char packet_ready; /** * \brief * @@ -80,11 +75,6 @@ typedef struct TDynamixelSlave * */ unsigned char current_rx_buffer; - /** - * \brief - * - */ - unsigned char rx_dma_mode; /** * \brief * @@ -115,21 +105,6 @@ typedef struct TDynamixelSlave * */ unsigned char (*on_relay)(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt); - /** - * \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); /** * \brief * diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c index fdb179c35204ac7f575aaac4cb9a60f01caa14e8..c48e5003a7c04909f303ac1e53ddbd0135ca69d5 100644 --- a/dynamixel_base/src/dynamixel_slave.c +++ b/dynamixel_base/src/dynamixel_slave.c @@ -1,67 +1,13 @@ #include "dynamixel_slave.h" // communication functions -void dyn_slave_receive_cb(TDynamixelSlave *slave) -{ - unsigned char *buffer; - - if(slave!=0x00000000) - { - if(slave->rx_dma_mode==0x01) - { - if(slave->current_rx_buffer==0) - { - buffer=slave->rx_buffer2; - slave->current_rx_buffer=1; - } - else - { - buffer=slave->rx_buffer1; - slave->current_rx_buffer=0; - } - slave->rx_dma_mode=0x00; - if(slave->hal_dev!=0x00000000) - { - if(slave->version==DYN_VER1) - slave->rx_irq(slave->hal_dev,buffer,4); - else - slave->rx_irq(slave->hal_dev,buffer,7); - } - time_set_timeout(&slave->time,slave->rx_timeout_ms*1000); - slave->packet_ready=0x01; - } - else - { - if(slave->current_rx_buffer==0) - buffer=slave->rx_buffer1; - else - buffer=slave->rx_buffer2; - slave->rx_dma_mode=0x01; - if(slave->version==DYN_VER1) - { - if(buffer[0]!=0xFF) return; - if(buffer[1]!=0xFF) return; - if(slave->hal_dev!=0x00000000) - slave->rx_dma(slave->hal_dev,&buffer[4],buffer[3]); - } - else - { - if(buffer[0]!=0xFF) return; - if(buffer[1]!=0xFF) return; - if(buffer[2]!=0xFD) return; - if(slave->hal_dev!=0x00000000) - slave->rx_dma(slave->hal_dev,&buffer[7],buffer[5]+(buffer[6]<<8)); - } - } - } -} - void dyn_slave_send_cb(TDynamixelSlave *slave) { if(slave!=0x00000000) { // enable tx interrupts slave->set_rx_mode(); + time_set_timeout(&slave->time,slave->rx_timeout_ms*1000); } } @@ -142,21 +88,61 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u } } +unsigned char dyn_slave_check_new_packet(TDynamixelSlave *slave) +{ + unsigned char *buffer; + + if(slave!=0x00000000) + { + if(slave->current_rx_buffer==0) + buffer=slave->rx_buffer1; + else + buffer=slave->rx_buffer2; + if(slave->version==DYN_VER1) + { + if(buffer[0]!=0xFF) return 0x00; + if(buffer[1]!=0xFF) return 0x00; + if(dyn_check_checksum(buffer)!=0xFF) return 0x00; + return 0x01; + } + else + { + if(buffer[0]!=0xFF) return 0x00; + if(buffer[1]!=0xFF) return 0x00; + if(buffer[2]!=0xFD) return 0x00; + if(dyn2_check_checksum(buffer)!=0x01) return 0x00; + return 0x01; + } + } + else + return 0x00; +} + void dyn_slave_loop(TDynamixelSlave *slave) { - unsigned char i,send_status,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN]; + unsigned char i,send_status=0xFF,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN]; unsigned short int length; unsigned char *buffer; if(slave!=0x00000000) { - if(slave->packet_ready)// check if a new instruction packet has been received + if(dyn_slave_check_new_packet(slave))// check if a new instruction packet has been received { - slave->packet_ready=0x00; + // cancel current DMA transfer + slave->abort_dma(slave->hal_dev); + // change current_buffer if(slave->current_rx_buffer==0) - buffer=slave->rx_buffer2; - else + { buffer=slave->rx_buffer1; + slave->current_rx_buffer=1; + slave->rx_dma(slave->hal_dev,slave->rx_buffer2,MAX_DYN_SLAVE_RX_BUFFER_LEN); + } + else + { + buffer=slave->rx_buffer2; + slave->current_rx_buffer=0; + slave->rx_dma(slave->hal_dev,slave->rx_buffer1,MAX_DYN_SLAVE_RX_BUFFER_LEN); + } // check address for(i=0;i<slave->num_slave_devices;i++) { @@ -179,7 +165,12 @@ void dyn_slave_loop(TDynamixelSlave *slave) else slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7); } + else + dyn_slave_send_cb(slave); } + // erase header of the current buffer + for(i=0;i<4;i++) + buffer[i]=0x00; } else { @@ -187,18 +178,14 @@ void dyn_slave_loop(TDynamixelSlave *slave) { if(slave->hal_dev!=0x00000000) { - /* cancel any IRQ or DMA reception */ - slave->abort_irq(slave->hal_dev); + // cancel DMA reception slave->abort_dma(slave->hal_dev); - /* enable reception by IRQ */ + // enable reception by DMa if(slave->current_rx_buffer==0) buffer=slave->rx_buffer1; else buffer=slave->rx_buffer2; - if(slave->version==DYN_VER1) - slave->rx_irq(slave->hal_dev,buffer,4); - else - slave->rx_irq(slave->hal_dev,buffer,7); + slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN); } time_set_timeout(&slave->time,slave->rx_timeout_ms*1000); } @@ -214,20 +201,15 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *sc slave->hal_dev=hal_dev; slave->version=version; slave->current_rx_buffer=0; - slave->rx_dma_mode=0x00; /* initialize the internal callbacks */ slave->set_tx_mode=dummy_dyn_slave_set_tx_mode; slave->set_rx_mode=dummy_dyn_slave_set_rx_mode; slave->set_baudrate=dummy_dyn_slave_set_baudrate; slave->on_relay=dummy_dyn_slave_on_relay; - slave->tx_irq=dummy_dyn_slave_tx_irq; - slave->rx_irq=dummy_dyn_slave_rx_irq; - slave->abort_irq=dummy_dyn_slave_abort_irq; slave->tx_dma=dummy_dyn_slave_tx_dma; slave->rx_dma=dummy_dyn_slave_rx_dma; slave->abort_dma=dummy_dyn_slave_abort_dma; /* initialize internal variables */ - slave->packet_ready=0x00; slave->rx_timeout_ms=50; /* initialize slave devices */ slave->num_slave_devices=0; @@ -259,10 +241,7 @@ void dyn_slave_start(TDynamixelSlave *slave) buffer=slave->rx_buffer1; else buffer=slave->rx_buffer2; - if(slave->version==DYN_VER1) - slave->rx_irq(slave->hal_dev,buffer,4); - else - slave->rx_irq(slave->hal_dev,buffer,7); + slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN); } /* start timeout */ time_set_timeout(&slave->time,slave->rx_timeout_ms*1000); @@ -273,10 +252,7 @@ void dyn_slave_stop(TDynamixelSlave *slave) if(slave->scheduler!=0x00000000) scheduler_disable_channel(slave->scheduler,slave->sch_channel); if(slave->hal_dev!=0x00000000) - { - slave->abort_irq(slave->hal_dev); slave->abort_dma(slave->hal_dev); - } /* cancel timeout */ time_cancel_timeout(&slave->time); }