diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h index d19c55cebca0f7822327cb58d8423a4188fc6710..9254389dae21c86327d034ad9c377f0fc58c5c09 100644 --- a/dynamixel_base/include/dynamixel_slave.h +++ b/dynamixel_base/include/dynamixel_slave.h @@ -64,17 +64,32 @@ typedef struct TDynamixelSlave * \brief * */ - unsigned char rx_buffer1[MAX_DYN_SLAVE_RX_BUFFER_LEN]; + unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN]; /** * \brief * */ - unsigned char rx_buffer2[MAX_DYN_SLAVE_RX_BUFFER_LEN]; + unsigned short int rx_buffer_read_ptr; /** * \brief * */ - unsigned char current_rx_buffer; + unsigned short int rx_buffer_write_ptr; + /** + * \brief + * + */ + unsigned char rx_state; + /** + * \brief + * + */ + unsigned short int rx_current_length; + /** + * \brief + * + */ + unsigned short int rx_total_length; /** * \brief * @@ -120,6 +135,11 @@ typedef struct TDynamixelSlave * */ unsigned char (*abort_dma)(void *hal_dev); + /** + * \brief + * + */ + unsigned short int (*get_rx_num_data)(void); }TDynamixelSlave; /* public functions */ diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c index 65d7bc4ddb133cd3f289932be97e7dd49d33a6e9..54937ed1472fd45ffc00ef11415febe5077d963b 100644 --- a/dynamixel_base/src/dynamixel_slave.c +++ b/dynamixel_base/src/dynamixel_slave.c @@ -11,6 +11,14 @@ void dyn_slave_send_cb(TDynamixelSlave *slave) } } +void dyn_slave_receive_cb(TDynamixelSlave *slave) +{ + if(slave!=0x00000000) + { + slave->rx_dma(slave->hal_dev,slave->rx_buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN); + } +} + void dummy_dyn_slave_set_tx_mode(void) { @@ -46,6 +54,11 @@ unsigned char dummy_dyn_slave_abort_dma(void *hal_dev) return DYN_SUCCESS; } +unsigned short int dummy_dyn_slave_get_rx_num_data(void) +{ + return 0; +} + void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,unsigned char error,unsigned short int length, unsigned char *data,unsigned char return_delay) { if(slave!=0x00000000) @@ -73,75 +86,164 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u } } -unsigned char dyn_slave_check_new_packet(TDynamixelSlave *slave) +unsigned short int dyn_slave_get_num_data(TDynamixelSlave *slave) { - unsigned char *buffer; + unsigned short int num; + + slave->rx_buffer_write_ptr=slave->get_rx_num_data(); + + if(slave->rx_buffer_write_ptr>=slave->rx_buffer_read_ptr) + num=slave->rx_buffer_write_ptr-slave->rx_buffer_read_ptr; + else + num=(MAX_DYN_SLAVE_RX_BUFFER_LEN-slave->rx_buffer_read_ptr)+slave->rx_buffer_write_ptr; + + return num; +} + +void dyn_slave_increment_rx_ptr(TDynamixelSlave *slave) +{ + if(slave->rx_buffer_read_ptr==(MAX_DYN_SLAVE_RX_BUFFER_LEN-1)) + slave->rx_buffer_read_ptr=0; + else + slave->rx_buffer_read_ptr++; +} + +unsigned char dyn_slave_get_next_packet(TDynamixelSlave *slave,unsigned char *packet) +{ + unsigned char byte,new_packet=0x00; + unsigned short int i,num=0; if(slave!=0x00000000) { - if(slave->current_rx_buffer==0) - buffer=slave->rx_buffer1; - else - buffer=slave->rx_buffer2; - if(slave->version==DYN_VER1) + num=dyn_slave_get_num_data(slave); +/* + if(num==0) { - if(buffer[0]!=0xFF) return 0x00; - if(buffer[1]!=0xFF) return 0x00; - if(dyn_check_checksum(buffer)!=0xFF) return 0x00; - return 0x01; + if(time_is_timeout(&slave->time)) + { + slave->rx_state=0; + slave->rx_current_length=0; + slave->rx_total_length=0; + } } else + time_set_timeout(&slave->time,slave->rx_timeout_ms*1000); +*/ + for(i=0;i<num;i++) { - if(buffer[0]!=0xFF) return 0x00; - if(buffer[1]!=0xFF) return 0x00; - if(buffer[2]!=0xFD) return 0x00; - if(dyn2_get_length(buffer)>MAX_DYN_SLAVE_RX_BUFFER_LEN) return 0x00; - if(dyn2_check_checksum(buffer)!=0x01) return 0x00; - return 0x01; + byte=slave->rx_buffer[slave->rx_buffer_read_ptr]; + switch(slave->rx_state) + { + case 0: if(byte==0xFF) + { + slave->rx_state++; + packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + } + break; + case 1: if(byte==0xFF) + { + slave->rx_state++; + packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + } + else + { + slave->rx_state=0; + slave->rx_current_length=0; + } + break; + case 2: if(byte==0xFD)// version 2 header + { + if(slave->version==DYN_VER2)// the module is configured for version 2 + { + slave->rx_state++; + packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + } + else + { + slave->rx_state=0; + slave->rx_current_length=0; + } + } + else + { + if(byte!=0xFF) + { + slave->rx_state++; + packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + } + } + break; + case 3: packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + if(slave->version==DYN_VER1) + { + slave->rx_total_length=byte; + slave->rx_state=7; + } + else + slave->rx_state++; + break; + case 4: packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + slave->rx_state++; + break; + case 5: packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + slave->rx_total_length=byte; + slave->rx_state++; + break; + case 6: packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + slave->rx_total_length+=(byte<<8); + slave->rx_state++; + break; + case 7: packet[slave->rx_current_length]=byte; + slave->rx_current_length++; + slave->rx_total_length--; + if(slave->rx_total_length==0) + { + new_packet=0x01; + slave->rx_state=0; + slave->rx_current_length=0; + } + break; + default: break; + } + dyn_slave_increment_rx_ptr(slave); + if(new_packet) + break; } } - else - return 0x00; + + return new_packet; } void dyn_slave_loop(TDynamixelSlave *slave) { - unsigned char send_status=0xFF,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN]; + unsigned char send_status=0xFF,error,data_in[MAX_DYN_SLAVE_RX_BUFFER_LEN],data_out[MAX_DYN_SLAVE_RX_BUFFER_LEN]; unsigned short int length,i; - unsigned char *buffer; if(slave!=0x00000000) { - if(dyn_slave_check_new_packet(slave))// check if a new instruction packet has been received + while(dyn_slave_get_next_packet(slave,data_in)) { - // cancel current DMA transfer - slave->abort_dma(slave->hal_dev); - // change current_buffer - if(slave->current_rx_buffer==0) - { - 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++) { if(slave->version==DYN_VER1) - send_status=dyn_v1_slave_loop(slave->slave_devices[i],buffer,&error,(unsigned char *)&length,data); + send_status=dyn_v1_slave_loop(slave->slave_devices[i],data_in,&error,(unsigned char *)&length,data_out); else - send_status=dyn_v2_slave_loop(slave->slave_devices[i],buffer,&error,&length,data); + send_status=dyn_v2_slave_loop(slave->slave_devices[i],data_in,&error,&length,data_out); if(send_status==0x01) - dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay); + dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data_out,slave->slave_devices[i]->return_delay); } if(send_status==0xFF)// packet has not been processed { - if(slave->on_relay(slave->version,buffer,slave->tx_buffer)==DYN_SUCCESS) + if(slave->on_relay(slave->version,data_out,slave->tx_buffer)==DYN_SUCCESS) { // set the tx mode, if necessary slave->set_tx_mode(); @@ -154,27 +256,6 @@ void dyn_slave_loop(TDynamixelSlave *slave) else dyn_slave_send_cb(slave); } - // erase header of the current buffer - for(i=0;i<MAX_DYN_SLAVE_RX_BUFFER_LEN;i++) - buffer[i]=0x00; - } - else - { - if(time_is_timeout(&slave->time)) - { - if(slave->hal_dev!=0x00000000) - { - // cancel DMA reception - slave->abort_dma(slave->hal_dev); - // enable reception by DMa - if(slave->current_rx_buffer==0) - buffer=slave->rx_buffer1; - else - buffer=slave->rx_buffer2; - slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN); - } - time_set_timeout(&slave->time,slave->rx_timeout_ms*1000); - } } } } @@ -186,7 +267,11 @@ 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_buffer_read_ptr=0; + slave->rx_buffer_write_ptr=0; + slave->rx_state=0; + slave->rx_current_length=0; + slave->rx_total_length=0; /* initialize the internal callbacks */ slave->set_tx_mode=dummy_dyn_slave_set_tx_mode; slave->set_rx_mode=dummy_dyn_slave_set_rx_mode; @@ -217,17 +302,13 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *sc void dyn_slave_start(TDynamixelSlave *slave) { - unsigned char *buffer; - if(slave->scheduler!=0x00000000) scheduler_enable_channel(slave->scheduler,slave->sch_channel); if(slave->hal_dev!=0x00000000) { - if(slave->current_rx_buffer==0) - buffer=slave->rx_buffer1; - else - buffer=slave->rx_buffer2; - slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN); + slave->rx_dma(slave->hal_dev,slave->rx_buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN); + slave->rx_buffer_read_ptr=0; + slave->rx_buffer_write_ptr=0; } /* start timeout */ time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);