diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 350378647e3d9eb8694c4f6fe6455c76145ec4e7..badb06401d5f55d74a1cb5b9e7f5c76cbf1f28cd 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -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
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index a3415844ee740f710fbb677d8a0a95a6bd08da41..fce15f76604c0ce607eb31edc98c5d0a818d2e5f 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -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 */
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
index 68c122281b01a4b3d0f9fbc6f5770e6ea8346c21..3b300702be8e86d4bcabf04128782530d1f53336 100644
--- a/dynamixel_base/src/dynamixel_slave_device.c
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -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;
+ }
}
}
}