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

Modifications to better use the HAL interface of the serial port.

parent b8685468
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
+ 63
− 23
View file @ 987e9d9d
......@@ -5,7 +5,7 @@
extern "C" {
#endif
#include "comm.h"
#include "stm32_time.h"
#include "dynamixel.h"
#include "dynamixel2.h"
#include "scheduler.h"
......@@ -29,21 +29,22 @@ extern "C" {
*/
typedef struct TDynamixelSlave
{
TTime time;
/**
* \brief
*
*/
TScheduler *scheduler;
void *hal_dev;
/**
* \brief
*
*/
sched_channel_t sch_channel;
TScheduler *scheduler;
/**
* \brief
*
*/
TComm *comm_dev;
sched_channel_t sch_channel;
/**
* \brief
*
......@@ -58,32 +59,32 @@ typedef struct TDynamixelSlave
* \brief
*
*/
unsigned short int received_bytes;
unsigned short int rx_timeout_ms;
/**
* \brief
*
*/
unsigned short int rx_timeout_ms;
unsigned char tx_buffer[MAX_DYN_SLAVE_TX_BUFFER_LEN];
/**
* \brief
*
*/
unsigned short int op_length;
unsigned char rx_buffer1[MAX_DYN_SLAVE_RX_BUFFER_LEN];
/**
* \brief
*
*/
unsigned char tx_buffer[MAX_DYN_SLAVE_TX_BUFFER_LEN];
unsigned char rx_buffer2[MAX_DYN_SLAVE_RX_BUFFER_LEN];
/**
* \brief
*
*/
unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
unsigned char current_rx_buffer;
/**
* \brief
*
*/
unsigned char user_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
unsigned char rx_dma_mode;
/**
* \brief
*
......@@ -108,12 +109,42 @@ typedef struct TDynamixelSlave
* \brief
*
*/
void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate);
void (*set_baudrate)(void *hal_dev,unsigned int baudrate);
/**
* \brief
*
*/
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
*
*/
unsigned char (*tx_dma)(void *hal_dev, const unsigned char *data, unsigned short int length);
/**
* \brief
*
*/
unsigned char (*rx_dma)(void *hal_dev, const unsigned char *data, unsigned short int length);
/**
* \brief
*
*/
unsigned char (*abort_dma)(void *hal_dev);
}TDynamixelSlave;
/* public functions */
......@@ -121,7 +152,19 @@ typedef struct TDynamixelSlave
* \brief
*
*/
unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version);
void dyn_slave_receive_cb(TDynamixelSlave *dyn_slave);
/**
* \brief
*
*/
void dyn_slave_send_cb(TDynamixelSlave *slave);
/**
* \brief
*
*/
unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version);
/**
* \brief
*
......@@ -134,7 +177,7 @@ unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice
static inline void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
{
if(slave!=0x00000000)
slave->set_baudrate(slave->comm_dev,baudrate);
slave->set_baudrate(slave->hal_dev,baudrate);
}
/**
* \brief
......@@ -160,21 +203,18 @@ static inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
* \brief
*
*/
static inline void dyn_slave_start(TDynamixelSlave *slave)
{
if(slave->scheduler!=0x00000000)
scheduler_enable_channel(slave->scheduler,slave->sch_channel);
}
void dyn_slave_start(TDynamixelSlave *slave);
/**
* \brief
*
*/
static inline void dyn_slave_stop(TDynamixelSlave *slave)
{
if(slave->scheduler!=0x00000000)
scheduler_disable_channel(slave->scheduler,slave->sch_channel);
}
void dyn_slave_stop(TDynamixelSlave *slave);
/**
* \brief
*
*/
void dyn_slave_loop(TDynamixelSlave *slave);
#ifdef __cplusplus
......
dynamixel_base/src/dynamixel_slave.c
+ 151
− 120
View file @ 987e9d9d
#include "dynamixel_slave.h"
// communication functions
unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte)
void dyn_slave_receive_cb(TDynamixelSlave *slave)
{
if(dyn_slave!=0x00000000)
unsigned char *buffer;
if(slave!=0x00000000)
{
switch(dyn_slave->received_bytes)
if(slave->rx_dma_mode==0x01)
{
case 0: if(byte==0xFF)
{
dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
}
if(dyn_slave->comm_dev->time!=0x00000000)
time_set_timeout(dyn_slave->comm_dev->time,dyn_slave->rx_timeout_ms*1000);
break;
case 1: if(byte==0xFF)
{
dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
}
else
dyn_slave->received_bytes--;
break;
case 2: if(byte==0xFD)// version 2 header
{
if(dyn_slave->version==DYN_VER2)// the module is configured for version 2
{
dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
}
else
dyn_slave->received_bytes=0;// ignore packet and restart synchronization
}
else if(byte!=0xFF)
{
dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
}
break;
case 3: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
if(dyn_slave->version==DYN_VER1)
{
dyn_slave->op_length=byte;
dyn_slave->received_bytes=0;
/* finish reception by IRQ */
comm_cancel_irq_receive(dyn_slave->comm_dev);
/* enable dma RX */
comm_receive_dma(dyn_slave->comm_dev,&dyn_slave->rx_buffer[4],dyn_slave->op_length);
}
else
dyn_slave->received_bytes++;
break;
case 4: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
break;
case 5: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
dyn_slave->op_length=byte;
break;
case 6: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte;
dyn_slave->received_bytes++;
dyn_slave->op_length+=(byte<<8);
dyn_slave->received_bytes=0;
/* finish reception by IRQ */
comm_cancel_irq_receive(dyn_slave->comm_dev);
/* enable dma RX */
comm_receive_dma(dyn_slave->comm_dev,&dyn_slave->rx_buffer[7],dyn_slave->op_length);
break;
default: break;
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));
}
}
}
return 0x00;
}
unsigned char dyn_slave_dma_send_cb(TDynamixelSlave *dyn_slave)
void dyn_slave_send_cb(TDynamixelSlave *slave)
{
if(dyn_slave!=0x00000000)
if(slave!=0x00000000)
{
// enable tx interrupts
dyn_slave->set_rx_mode();
slave->set_rx_mode();
}
}
void dummy_dyn_slave_set_tx_mode(void)
{
return 0x00;
}
unsigned char dyn_slave_dma_receive_cb(TDynamixelSlave *dyn_slave)
void dummy_dyn_slave_set_rx_mode(void)
{
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;
}
}
void dummy_dyn_slave_set_baudrate(void *hal_dev,unsigned int baudrate)
{
return 0x00;
}
void dummy_dyn_slave_set_tx_mode(void)
unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
{
return DYN_NO_DEVICE;
}
unsigned char dummy_dyn_slave_tx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
void dummy_dyn_slave_set_rx_mode(void)
unsigned char dummy_dyn_slave_rx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_slave_tx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate)
unsigned char dummy_dyn_slave_rx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_slave_abort_irq(void *hal_dev)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
unsigned char dummy_dyn_slave_abort_dma(void *hal_dev)
{
return DYN_NO_DEVICE;
return DYN_SUCCESS;
}
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)
{
// cancel any transmission pending
comm_do_dma_send(slave->comm_dev);
if(return_delay>0)
if(slave->comm_dev->time!=0x00000000)
time_delay_us(slave->comm_dev->time,return_delay<<1);
time_delay_us(&slave->time,return_delay<<1);
if(slave->version==DYN_VER1)
{
// create the status packet
......@@ -143,7 +128,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
}
else
{
......@@ -152,7 +137,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
}
}
}
......@@ -161,75 +146,89 @@ void dyn_slave_loop(TDynamixelSlave *slave)
{
unsigned char i,send_status,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
{
slave->packet_ready=0x00;
if(slave->current_rx_buffer==0)
buffer=slave->rx_buffer2;
else
buffer=slave->rx_buffer1;
// 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],slave->user_buffer,&error,(unsigned char *)&length,data);
send_status=dyn_v1_slave_loop(slave->slave_devices[i],buffer,&error,(unsigned char *)&length,data);
else
send_status=dyn_v2_slave_loop(slave->slave_devices[i],slave->user_buffer,&error,&length,data);
send_status=dyn_v2_slave_loop(slave->slave_devices[i],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->user_buffer,slave->tx_buffer)==DYN_SUCCESS)
if(slave->on_relay(slave->version,buffer,slave->tx_buffer)==DYN_SUCCESS)
{
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
if(slave->version==DYN_VER1)
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
else
comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
}
}
}
else
{
if(slave->comm_dev->time!=0x00000000)
if(time_is_timeout(&slave->time))
{
if(time_is_timeout(slave->comm_dev->time))
{
if(slave->hal_dev!=0x00000000)
{
/* cancel any IRQ or DMA reception */
comm_cancel_irq_receive(slave->comm_dev);
comm_cancel_dma_receive(slave->comm_dev);
slave->received_bytes=0;
slave->abort_irq(slave->hal_dev);
slave->abort_dma(slave->hal_dev);
/* enable reception by IRQ */
comm_receive_irq(slave->comm_dev,0);
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);
}
time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
}
}
}
}
/* public functions */
unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version)
unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version)
{
unsigned char i;
/* assign communication functions */
dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb;
dev->dma_send_cb=(unsigned char (*)(void *))dyn_slave_dma_send_cb;
dev->dma_receive_cb=(unsigned char (*)(void *))dyn_slave_dma_receive_cb;
slave->comm_dev=dev;
slave->hal_dev=hal_dev;
slave->version=version;
dev->data=slave;
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;
slave->received_bytes=0x00;
/* initialize slave devices */
slave->num_slave_devices=0;
for(i=0;i<MAX_NUM_SLAVE_DEVICES;i++)
......@@ -244,12 +243,44 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *sched
slave->scheduler=scheduler;
slave->set_rx_mode();
/* enable reception by IRQ */
comm_receive_irq(slave->comm_dev,0);
return 0x01;
}
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;
if(slave->version==DYN_VER1)
slave->rx_irq(slave->hal_dev,buffer,4);
else
slave->rx_irq(slave->hal_dev,buffer,7);
}
/* start timeout */
time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
}
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);
}
unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device)
{
if(slave!=0x00000000)
......
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