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

Modified the dynamixel master module to better use the HAL interface.

parent 987e9d9d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_base/include/dynamixel_master.h
+ 54
− 7
View file @ 8ed22672
...@@ -5,7 +5,7 @@ ...@@ -5,7 +5,7 @@
extern "C" { extern "C" {
#endif #endif
#include "comm.h" #include "stm32_time.h"
#include "dynamixel.h" #include "dynamixel.h"
#include "dynamixel2.h" #include "dynamixel2.h"
...@@ -27,17 +27,17 @@ typedef struct ...@@ -27,17 +27,17 @@ typedef struct
* \brief * \brief
* *
*/ */
TComm *comm_dev; TTime time;
/** /**
* \brief * \brief
* *
*/ */
TDynVersion version; void *hal_dev;
/** /**
* \brief * \brief
* *
*/ */
unsigned short int op_length; TDynVersion version;
/** /**
* \brief * \brief
* *
...@@ -62,7 +62,17 @@ typedef struct ...@@ -62,7 +62,17 @@ typedef struct
* \brief * \brief
* *
*/ */
unsigned short int received_bytes; unsigned char rx_dma_mode;
/**
* \brief
*
*/
volatile unsigned char tx_done;
/**
* \brief
*
*/
unsigned short int sync_bulk_address;
/** /**
* \brief * \brief
* *
...@@ -102,7 +112,32 @@ typedef struct ...@@ -102,7 +112,32 @@ typedef struct
* \brief * \brief
* *
*/ */
unsigned char packet_bytes; 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);
}TDynamixelMaster; }TDynamixelMaster;
/* public functions */ /* public functions */
...@@ -110,7 +145,19 @@ typedef struct ...@@ -110,7 +145,19 @@ typedef struct
* \brief * \brief
* *
*/ */
void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version); void dyn_master_receive_cb(TDynamixelMaster *master);
/**
* \brief
*
*/
void dyn_master_send_cb(TDynamixelMaster *master);
/**
* \brief
*
*/
void dyn_master_init(TDynamixelMaster *master,void *hal_dev,TDynVersion version);
/** /**
* \brief * \brief
* *
......
dynamixel_base/include/dynamixel_slave.h
+ 1
− 1
View file @ 8ed22672
...@@ -152,7 +152,7 @@ typedef struct TDynamixelSlave ...@@ -152,7 +152,7 @@ typedef struct TDynamixelSlave
* \brief * \brief
* *
*/ */
void dyn_slave_receive_cb(TDynamixelSlave *dyn_slave); void dyn_slave_receive_cb(TDynamixelSlave *slave);
/** /**
* \brief * \brief
......
dynamixel_base/src/dynamixel_master.c
+ 123
− 188
View file @ 8ed22672
#include "dynamixel_master.h" #include "dynamixel_master.h"
/* private functions */ /* private functions */
unsigned char dyn_master_irq_receive_cb(TDynamixelMaster *dyn_master,unsigned char byte) void dyn_master_receive_cb(TDynamixelMaster *master)
{ {
if(dyn_master!=0x00000000) unsigned short int length;
if(master!=0x00000000)
{ {
if(dyn_master->comm_dev->time!=0x00000000) if(master->rx_dma_mode==0x01)
time_set_timeout(dyn_master->comm_dev->time,dyn_master->rx_timeout_ms*1000);
switch(dyn_master->packet_bytes)
{ {
case 0: if(byte==0xFF) master->rx_dma_mode=0x00;
{ master->rx_num_packets--;
dyn_master->rx_buffer[dyn_master->received_bytes]=byte; if(master->rx_num_packets==0x00)
dyn_master->received_bytes++; {
dyn_master->packet_bytes++; time_cancel_timeout(&master->time);
} master->packet_ready=0x01;
break; }
case 1: if(byte==0xFF) else
{ {
dyn_master->rx_buffer[dyn_master->received_bytes]=byte; if(master->version==DYN_VER1)
dyn_master->received_bytes++; master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],4);
dyn_master->packet_bytes++; else
} master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],7);
else }
{
dyn_master->received_bytes--;
dyn_master->packet_bytes--;
}
break;
case 2: if(byte==0xFD)// version 2 header
{
if(dyn_master->version==DYN_VER2)// the module is configured for version 2
{
dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
dyn_master->packet_bytes++;
}
else
{
dyn_master->received_bytes-=2;// ignore packet and restart synchronization
dyn_master->packet_bytes=0;
}
}
else if(byte!=0xFF)
{
dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
dyn_master->packet_bytes++;
}
break;
case 3: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
if(dyn_master->version==DYN_VER1)
{
dyn_master->op_length=byte;
/* finish reception by IRQ */
comm_cancel_irq_receive(dyn_master->comm_dev);
/* enable dma RX */
comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[dyn_master->received_bytes+1],dyn_master->op_length);
dyn_master->received_bytes+=dyn_master->op_length+1;
dyn_master->packet_bytes=0;
}
else
{
dyn_master->received_bytes++;
dyn_master->packet_bytes++;
}
break;
case 4: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
dyn_master->packet_bytes++;
break;
case 5: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
dyn_master->op_length=byte;
dyn_master->packet_bytes++;
break;
case 6: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->op_length+=(byte<<8);
/* finish reception by IRQ */
comm_cancel_irq_receive(dyn_master->comm_dev);
/* enable dma RX */
comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[dyn_master->received_bytes+1],dyn_master->op_length);
dyn_master->received_bytes+=dyn_master->op_length+1;
dyn_master->packet_bytes=0;
break;
default: break;
} }
}
return 0x00;
}
unsigned char dyn_master_dma_send_cb(TDynamixelMaster *dyn_master)
{
if(dyn_master!=0x00000000)
{
dyn_master->set_rx_mode();
if(dyn_master->rx_no_answer)
dyn_master->rx_no_answer=0x00;
else else
{ {
/* enable RX interrupts */ master->rx_dma_mode=0x01;
comm_receive_irq(dyn_master->comm_dev,0); if(master->version==DYN_VER1)
{
if(master->rx_buffer[0]!=0xFF) return;
if(master->rx_buffer[1]!=0xFF) return;
if(master->hal_dev!=0x00000000)
{
length=master->rx_buffer[master->sync_bulk_address+3];
master->rx_dma(master->hal_dev,&master->rx_buffer[master->sync_bulk_address+4],length);
}
}
else
{
if(master->rx_buffer[0]!=0xFF) return;
if(master->rx_buffer[1]!=0xFF) return;
if(master->rx_buffer[2]!=0xFD) return;
if(master->hal_dev!=0x00000000)
{
length=master->rx_buffer[master->sync_bulk_address+5]+(master->rx_buffer[master->sync_bulk_address+6]<<8);
master->rx_dma(master->hal_dev,&master->rx_buffer[master->sync_bulk_address+7],length);
}
}
master->sync_bulk_address+=length;
} }
} }
return 0x00;
} }
unsigned char dyn_master_dma_receive_cb(TDynamixelMaster *dyn_master) void dyn_master_send_cb(TDynamixelMaster *master)
{ {
if(dyn_master!=0x00000000) if(master!=0x00000000)
{ {
dyn_master->rx_num_packets--; master->tx_done=0x01;
if(dyn_master->rx_num_packets==0x00) master->set_rx_mode();
{ if(master->rx_no_answer)
if(dyn_master->comm_dev->time!=0x00000000) master->rx_no_answer=0x00;
time_cancel_timeout(dyn_master->comm_dev->time);
dyn_master->packet_ready=0x01;
dyn_master->received_bytes=0;
}
else else
{ {
/* enable RX interrupts */ if(master->version==DYN_VER1)
comm_receive_irq(dyn_master->comm_dev,0); master->rx_irq(master->hal_dev,master->rx_buffer,4);
// dyn_master->packet_ready=0x01; else
master->rx_irq(master->hal_dev,master->rx_buffer,7);
} }
} }
return 0x00;
} }
void dummy_dyn_master_set_tx_mode(void) void dummy_dyn_master_set_tx_mode(void)
...@@ -151,18 +91,45 @@ void dummy_dyn_master_disable_power(void) ...@@ -151,18 +91,45 @@ void dummy_dyn_master_disable_power(void)
} }
unsigned char dummy_dyn_master_tx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_master_rx_irq(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_master_tx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_master_rx_dma(void *hal_dev, const unsigned char *data, unsigned short int length)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_master_abort_irq(void *hal_dev)
{
return DYN_SUCCESS;
}
unsigned char dummy_dyn_master_abort_dma(void *hal_dev)
{
return DYN_SUCCESS;
}
unsigned char dyn_master_wait_transmission(TDynamixelMaster *master) unsigned char dyn_master_wait_transmission(TDynamixelMaster *master)
{ {
unsigned char error; unsigned char error;
if(master!=0x00000000) if(master!=0x00000000)
{ {
while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY); while(master->tx_done==0x00);
if(error==COMM_SUCCESS) return DYN_SUCCESS;
return DYN_SUCCESS;
else
return DYN_COMM_ERROR;
} }
else else
...@@ -175,23 +142,13 @@ unsigned char dyn_master_is_transmission_done(TDynamixelMaster *master) ...@@ -175,23 +142,13 @@ unsigned char dyn_master_is_transmission_done(TDynamixelMaster *master)
if(master!=0x00000000) if(master!=0x00000000)
{ {
error=comm_is_send_done(master->comm_dev); if(master->tx_done==0x00)
if(error==COMM_BUSY)
return DYN_BUSY; return DYN_BUSY;
else if(error==COMM_SUCCESS) else
{ {
if(master->comm_dev->time!=0x00000000) time_set_timeout(&master->time,master->rx_timeout_ms*1000);
{
// start the new timeout
time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
}
return DYN_SUCCESS; return DYN_SUCCESS;
} }
else
{
master->set_rx_mode();
return DYN_COMM_ERROR;
}
} }
else else
return DYN_COMM_ERROR; return DYN_COMM_ERROR;
...@@ -209,12 +166,12 @@ unsigned char dyn_master_send(TDynamixelMaster *master) ...@@ -209,12 +166,12 @@ unsigned char dyn_master_send(TDynamixelMaster *master)
// set the DMA transfer // set the DMA transfer
if(master->version==DYN_VER1) if(master->version==DYN_VER1)
{ {
comm_send_dma(master->comm_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4); master->tx_dma(master->hal_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4);
return DYN_SUCCESS; return DYN_SUCCESS;
} }
else else
{ {
comm_send_dma(master->comm_dev,master->tx_buffer,dyn2_get_length(master->tx_buffer)+7); master->tx_dma(master->hal_dev,master->tx_buffer,dyn2_get_length(master->tx_buffer)+7);
return DYN_SUCCESS; return DYN_SUCCESS;
} }
} }
...@@ -226,23 +183,17 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master) ...@@ -226,23 +183,17 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
{ {
if(master!=0x00000000) if(master!=0x00000000)
{ {
if(master->comm_dev->time!=0x00000000) // start the new timeout
{ time_set_timeout(&master->time,master->rx_timeout_ms*1000);
// start the new timeout
time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
}
// wait for the status packet // wait for the status packet
while(!master->packet_ready) while(!master->packet_ready)
{ {
if(master->comm_dev->time!=0x00000000) if(time_is_timeout(&master->time))
{ {
if(time_is_timeout(master->comm_dev->time)) /* cancel any IRQ or DMA reception */
{ master->abort_irq(master->hal_dev);
comm_cancel_irq_receive(master->comm_dev); master->abort_dma(master->hal_dev);
comm_cancel_dma_receive(master->comm_dev); return DYN_TIMEOUT;
master->received_bytes=0x00;
return DYN_TIMEOUT;
}
} }
} }
master->packet_ready=0x00; master->packet_ready=0x00;
...@@ -258,17 +209,12 @@ unsigned char dyn_master_is_reception_done(TDynamixelMaster *master) ...@@ -258,17 +209,12 @@ unsigned char dyn_master_is_reception_done(TDynamixelMaster *master)
{ {
if(!master->packet_ready) if(!master->packet_ready)
{ {
if(master->comm_dev->time!=0x00000000) if(time_is_timeout(&master->time))
{ {
if(time_is_timeout(master->comm_dev->time)) /* cancel any IRQ or DMA reception */
{ master->abort_irq(master->hal_dev);
comm_cancel_irq_receive(master->comm_dev); master->abort_dma(master->hal_dev);
comm_cancel_dma_receive(master->comm_dev); return DYN_TIMEOUT;
master->received_bytes=0x00;
return DYN_TIMEOUT;
}
else
return DYN_BUSY;
} }
else else
return DYN_BUSY; return DYN_BUSY;
...@@ -484,16 +430,10 @@ unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned ch ...@@ -484,16 +430,10 @@ unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned ch
if(master!=0x00000000) if(master!=0x00000000)
{ {
error=comm_is_send_done(master->comm_dev); if(master->tx_done==0x00)
if(error==COMM_BUSY)
return DYN_BUSY; return DYN_BUSY;
else if(error==COMM_SUCCESS) else
return DYN_SUCCESS; return DYN_SUCCESS;
else
{
master->set_rx_mode();
return DYN_COMM_ERROR;
}
} }
else else
return error; return error;
...@@ -516,6 +456,7 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char ...@@ -516,6 +456,7 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
master->rx_no_answer=0x00; master->rx_no_answer=0x00;
// enable transmission // enable transmission
master->set_tx_mode(); master->set_tx_mode();
master->sync_bulk_address=0x0000;
// send the data // send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS) if((error=dyn_master_send(master))!=DYN_SUCCESS)
master->set_rx_mode(); master->set_rx_mode();
...@@ -619,23 +560,12 @@ unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned ch ...@@ -619,23 +560,12 @@ unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned ch
if(master!=0x00000000) if(master!=0x00000000)
{ {
if(master->version==DYN_VER2) if(master->tx_done==0x00)
{ return DYN_BUSY;
error=comm_is_send_done(master->comm_dev); else
if(error==COMM_BUSY) return DYN_SUCCESS;
return DYN_BUSY;
else if(error==COMM_SUCCESS)
return DYN_SUCCESS;
else
{
master->set_rx_mode();
return DYN_COMM_ERROR;
}
}
else
return DYN_INST_ERROR;
} }
else else
return error; return error;
} }
...@@ -664,6 +594,7 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char ...@@ -664,6 +594,7 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
master->rx_no_answer=0x00; master->rx_no_answer=0x00;
// enable transmission // enable transmission
master->set_tx_mode(); master->set_tx_mode();
master->sync_bulk_address=0x0000;
// send the data // send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS) if((error=dyn_master_send(master))!=DYN_SUCCESS)
master->set_rx_mode(); master->set_rx_mode();
...@@ -746,28 +677,30 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha ...@@ -746,28 +677,30 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
} }
/* public functions */ /* public functions */
void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version) void dyn_master_init(TDynamixelMaster *master,void *hal_dev,TDynVersion version)
{ {
/* assign communication functions */ master->hal_dev=hal_dev;
dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_master_irq_receive_cb;
dev->dma_send_cb=(unsigned char (*)(void *))dyn_master_dma_send_cb;
dev->dma_receive_cb=(unsigned char (*)(void *))dyn_master_dma_receive_cb;
master->comm_dev=dev;
master->version=version; master->version=version;
dev->data=master; master->rx_dma_mode=0x00;
master->tx_done=0x01;
/* initialize the internal callbacks */ /* initialize the internal callbacks */
master->set_tx_mode=dummy_dyn_master_set_tx_mode; master->set_tx_mode=dummy_dyn_master_set_tx_mode;
master->set_rx_mode=dummy_dyn_master_set_rx_mode; master->set_rx_mode=dummy_dyn_master_set_rx_mode;
master->enable_power=dummy_dyn_master_enable_power; master->enable_power=dummy_dyn_master_enable_power;
master->disable_power=dummy_dyn_master_disable_power; master->disable_power=dummy_dyn_master_disable_power;
master->tx_irq=dummy_dyn_master_tx_irq;
master->rx_irq=dummy_dyn_master_rx_irq;
master->abort_irq=dummy_dyn_master_abort_irq;
master->tx_dma=dummy_dyn_master_tx_dma;
master->rx_dma=dummy_dyn_master_rx_dma;
master->abort_dma=dummy_dyn_master_abort_dma;
/* initialize internal variables */ /* initialize internal variables */
master->packet_ready=0x00; master->packet_ready=0x00;
master->received_bytes=0x00;
master->rx_timeout_ms=50; master->rx_timeout_ms=50;
master->rx_no_answer=0x00; master->rx_no_answer=0x00;
master->rx_num_packets=0x00; master->rx_num_packets=0x00;
master->return_level=return_all; master->return_level=return_all;
master->packet_bytes=0; master->sync_bulk_address;
master->set_rx_mode(); master->set_rx_mode();
} }
...@@ -1140,6 +1073,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un ...@@ -1140,6 +1073,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
master->rx_no_answer=0x00; master->rx_no_answer=0x00;
// enable transmission // enable transmission
master->set_tx_mode(); master->set_tx_mode();
master->sync_bulk_address=0x0000;
// send the data // send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS) if((error=dyn_master_send(master))!=DYN_SUCCESS)
{ {
...@@ -1206,6 +1140,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un ...@@ -1206,6 +1140,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
master->rx_no_answer=0x00; master->rx_no_answer=0x00;
// enable transmission // enable transmission
master->set_tx_mode(); master->set_tx_mode();
master->sync_bulk_address=0x0000;
// send the data // send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS) if((error=dyn_master_send(master))!=DYN_SUCCESS)
{ {
......
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