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

Made the use of the scheduler optional to execute the slave loop.

Added a user buffer to allow reception of new data while processing the current one.
Solved a bug in handlding error for bulk and sync write operations.
parent 3ddf64e6
No related branches found
No related tags found
No related merge requests found
......@@ -79,6 +79,11 @@ typedef struct TDynamixelSlave
*
*/
unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
/**
* \brief
*
*/
unsigned char user_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
/**
* \brief
*
......@@ -157,7 +162,8 @@ static inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
*/
static inline void dyn_slave_start(TDynamixelSlave *slave)
{
scheduler_enable_channel(slave->scheduler,slave->sch_channel);
if(slave->scheduler!=0x00000000)
scheduler_enable_channel(slave->scheduler,slave->sch_channel);
}
/**
* \brief
......@@ -165,9 +171,12 @@ static inline void dyn_slave_start(TDynamixelSlave *slave)
*/
static inline void dyn_slave_stop(TDynamixelSlave *slave)
{
scheduler_disable_channel(slave->scheduler,slave->sch_channel);
if(slave->scheduler!=0x00000000)
scheduler_disable_channel(slave->scheduler,slave->sch_channel);
}
void dyn_slave_loop(TDynamixelSlave *slave);
#ifdef __cplusplus
}
#endif
......
......@@ -88,11 +88,19 @@ unsigned char dyn_slave_dma_send_cb(TDynamixelSlave *dyn_slave)
unsigned char dyn_slave_dma_receive_cb(TDynamixelSlave *dyn_slave)
{
unsigned short int i,length;
if(dyn_slave!=0x00000000)
{
comm_receive_irq(dyn_slave->comm_dev,0);// reenable reception by IRQ
if(dyn_slave->comm_dev->time!=0x00000000)
time_cancel_timeout(dyn_slave->comm_dev->time);
if(dyn_slave->version==DYN_VER1)
length=dyn_slave->op_length+4;
else
length=dyn_slave->op_length+7;
for(i=0;i<length;i++)
dyn_slave->user_buffer[i]=dyn_slave->rx_buffer[i];
dyn_slave->packet_ready=0x01;
}
......@@ -163,15 +171,15 @@ void dyn_slave_loop(TDynamixelSlave *slave)
for(i=0;i<slave->num_slave_devices;i++)
{
if(slave->version==DYN_VER1)
send_status=dyn_v1_slave_loop(slave->slave_devices[i],slave->rx_buffer,&error,(unsigned char *)&length,data);
send_status=dyn_v1_slave_loop(slave->slave_devices[i],slave->user_buffer,&error,(unsigned char *)&length,data);
else
send_status=dyn_v2_slave_loop(slave->slave_devices[i],slave->rx_buffer,&error,&length,data);
send_status=dyn_v2_slave_loop(slave->slave_devices[i],slave->user_buffer,&error,&length,data);
if(send_status==0x01)
dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
}
if(send_status==0xFF)// packet has not been processed
{
if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
if(slave->on_relay(slave->version,slave->user_buffer,slave->tx_buffer)==DYN_SUCCESS)
{
// set the tx mode, if necessary
slave->set_tx_mode();
......@@ -228,9 +236,12 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *sched
slave->slave_devices[i]=0x00000000;
/* assigna a scheduler channel */
scheduler_set_channel(scheduler,ch,(void (*)(void *))dyn_slave_loop,1,slave);
if(scheduler!=0x00000000)
{
scheduler_set_channel(scheduler,ch,(void (*)(void *))dyn_slave_loop,1,slave);
slave->sch_channel=ch;
}
slave->scheduler=scheduler;
slave->sch_channel=ch;
slave->set_rx_mode();
/* enable reception by IRQ */
......
......@@ -177,11 +177,34 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
else
{
// send a checksum error answer
if(dyn_get_id(rx_buffer)!=DYN_BROADCAST_ID)
if(id!=DYN_BROADCAST_ID)
{
send_status=0x01;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
if(dyn_get_instruction(rx_buffer)==DYN_BULK_READ)
{
prev_id=dyn_bulk_read_id_present(rx_buffer,device->address,&address,length);
if(prev_id!=0xFF)
{
if(prev_id==0x00)// first device to answer
{
send_status=0x01;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
}
else// wait for the previous device in the sequence to send its data
{
device->sync_bulk_address=-1;
device->sync_bulk_length=-1;
device->sync_bulk_prev_id=prev_id;
device->bulk_read_pending=0x01;
}
}
}
else
{
send_status=0x01;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
}
}
}
}
......@@ -193,14 +216,22 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
{
device->bulk_read_pending=0x00;
send_status=0x01;
(*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
if((*error)==DYN_NO_ERROR)
(*length)=device->sync_bulk_length;
else
if(device->sync_bulk_address==-1 && device->sync_bulk_length==-1)
{
(*error)=DYN_INST_ERROR;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
}
else
{
(*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
if((*error)==DYN_NO_ERROR)
(*length)=device->sync_bulk_length;
else
{
(*error)=DYN_INST_ERROR;
(*length)=0;
}
}
}
}
else
......@@ -344,11 +375,34 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
else
{
// send a checksum error answer
if(dyn_get_id(rx_buffer)!=DYN_BROADCAST_ID)
if(dyn2_get_id(rx_buffer)!=DYN_BROADCAST_ID)
{
send_status=0x01;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
if(dyn2_get_instruction(rx_buffer)==DYN_BULK_READ || dyn2_get_instruction(rx_buffer)==DYN_SYNC_READ)
{
prev_id=dyn2_bulk_read_id_present(rx_buffer,device->address,&address,length);
if(prev_id!=0xFF)
{
if(prev_id==0x00)// first device to answer
{
send_status=0x01;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
}
else// wait for the previous device in the sequence to send its data
{
device->sync_bulk_address=-1;
device->sync_bulk_length=-1;
device->sync_bulk_prev_id=prev_id;
device->bulk_read_pending=0x01;
}
}
}
else
{
send_status=0x01;
(*error)=DYN_CHECKSUM_ERROR;
(*length)=0;
}
}
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment