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

Improved the communication using DMA. Not yet fully operational.

parent c52e682e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_base/include/dynamixel_slave.h
+ 23
− 3
View file @ cbe7d928
......@@ -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 */
......
dynamixel_base/src/dynamixel_slave.c
+ 151
− 70
View file @ cbe7d928
......@@ -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);
......
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