From 8ed226725937104f26dc730194fdd302b1370d7c Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sun, 19 May 2024 19:35:07 +0200
Subject: [PATCH] Modified the dynamixel master module to better use the HAL
interface.
---
dynamixel_base/include/dynamixel_master.h | 61 ++++-
dynamixel_base/include/dynamixel_slave.h | 2 +-
dynamixel_base/src/dynamixel_master.c | 311 +++++++++-------------
3 files changed, 178 insertions(+), 196 deletions(-)
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 64df2a8..740162c 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -5,7 +5,7 @@
extern "C" {
#endif
-#include "comm.h"
+#include "stm32_time.h"
#include "dynamixel.h"
#include "dynamixel2.h"
@@ -27,17 +27,17 @@ typedef struct
* \brief
*
*/
- TComm *comm_dev;
+ TTime time;
/**
* \brief
*
*/
- TDynVersion version;
+ void *hal_dev;
/**
* \brief
*
*/
- unsigned short int op_length;
+ TDynVersion version;
/**
* \brief
*
@@ -62,7 +62,17 @@ typedef struct
* \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
*
@@ -102,7 +112,32 @@ typedef struct
* \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;
/* public functions */
@@ -110,7 +145,19 @@ typedef struct
* \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
*
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 311b35c..203be76 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -152,7 +152,7 @@ typedef struct TDynamixelSlave
* \brief
*
*/
-void dyn_slave_receive_cb(TDynamixelSlave *dyn_slave);
+void dyn_slave_receive_cb(TDynamixelSlave *slave);
/**
* \brief
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index df84363..14163f0 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -1,134 +1,74 @@
#include "dynamixel_master.h"
/* 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)
- time_set_timeout(dyn_master->comm_dev->time,dyn_master->rx_timeout_ms*1000);
- switch(dyn_master->packet_bytes)
+ if(master->rx_dma_mode==0x01)
{
- case 0: if(byte==0xFF)
- {
- dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
- dyn_master->received_bytes++;
- dyn_master->packet_bytes++;
- }
- break;
- case 1: if(byte==0xFF)
- {
- dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
- dyn_master->received_bytes++;
- dyn_master->packet_bytes++;
- }
- 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;
+ master->rx_dma_mode=0x00;
+ master->rx_num_packets--;
+ if(master->rx_num_packets==0x00)
+ {
+ time_cancel_timeout(&master->time);
+ master->packet_ready=0x01;
+ }
+ else
+ {
+ if(master->version==DYN_VER1)
+ master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],4);
+ else
+ master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],7);
+ }
}
- }
-
- 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
{
- /* enable RX interrupts */
- comm_receive_irq(dyn_master->comm_dev,0);
+ master->rx_dma_mode=0x01;
+ 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--;
- if(dyn_master->rx_num_packets==0x00)
- {
- if(dyn_master->comm_dev->time!=0x00000000)
- time_cancel_timeout(dyn_master->comm_dev->time);
- dyn_master->packet_ready=0x01;
- dyn_master->received_bytes=0;
- }
+ master->tx_done=0x01;
+ master->set_rx_mode();
+ if(master->rx_no_answer)
+ master->rx_no_answer=0x00;
else
{
- /* enable RX interrupts */
- comm_receive_irq(dyn_master->comm_dev,0);
-// dyn_master->packet_ready=0x01;
+ if(master->version==DYN_VER1)
+ master->rx_irq(master->hal_dev,master->rx_buffer,4);
+ else
+ master->rx_irq(master->hal_dev,master->rx_buffer,7);
}
}
-
- return 0x00;
}
void dummy_dyn_master_set_tx_mode(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 error;
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;
- else
- return DYN_COMM_ERROR;
+ return DYN_SUCCESS;
}
else
@@ -175,23 +142,13 @@ unsigned char dyn_master_is_transmission_done(TDynamixelMaster *master)
if(master!=0x00000000)
{
- error=comm_is_send_done(master->comm_dev);
- if(error==COMM_BUSY)
+ if(master->tx_done==0x00)
return DYN_BUSY;
- else if(error==COMM_SUCCESS)
+ else
{
- if(master->comm_dev->time!=0x00000000)
- {
- // start the new timeout
- time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
- }
+ time_set_timeout(&master->time,master->rx_timeout_ms*1000);
return DYN_SUCCESS;
}
- else
- {
- master->set_rx_mode();
- return DYN_COMM_ERROR;
- }
}
else
return DYN_COMM_ERROR;
@@ -209,12 +166,12 @@ unsigned char dyn_master_send(TDynamixelMaster *master)
// set the DMA transfer
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;
}
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;
}
}
@@ -226,23 +183,17 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
{
if(master!=0x00000000)
{
- if(master->comm_dev->time!=0x00000000)
- {
- // start the new timeout
- time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
- }
+ // start the new timeout
+ time_set_timeout(&master->time,master->rx_timeout_ms*1000);
// wait for the status packet
while(!master->packet_ready)
{
- if(master->comm_dev->time!=0x00000000)
+ if(time_is_timeout(&master->time))
{
- if(time_is_timeout(master->comm_dev->time))
- {
- comm_cancel_irq_receive(master->comm_dev);
- comm_cancel_dma_receive(master->comm_dev);
- master->received_bytes=0x00;
- return DYN_TIMEOUT;
- }
+ /* cancel any IRQ or DMA reception */
+ master->abort_irq(master->hal_dev);
+ master->abort_dma(master->hal_dev);
+ return DYN_TIMEOUT;
}
}
master->packet_ready=0x00;
@@ -258,17 +209,12 @@ unsigned char dyn_master_is_reception_done(TDynamixelMaster *master)
{
if(!master->packet_ready)
{
- if(master->comm_dev->time!=0x00000000)
+ if(time_is_timeout(&master->time))
{
- if(time_is_timeout(master->comm_dev->time))
- {
- comm_cancel_irq_receive(master->comm_dev);
- comm_cancel_dma_receive(master->comm_dev);
- master->received_bytes=0x00;
- return DYN_TIMEOUT;
- }
- else
- return DYN_BUSY;
+ /* cancel any IRQ or DMA reception */
+ master->abort_irq(master->hal_dev);
+ master->abort_dma(master->hal_dev);
+ return DYN_TIMEOUT;
}
else
return DYN_BUSY;
@@ -484,16 +430,10 @@ unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned ch
if(master!=0x00000000)
{
- error=comm_is_send_done(master->comm_dev);
- if(error==COMM_BUSY)
+ if(master->tx_done==0x00)
return DYN_BUSY;
- else if(error==COMM_SUCCESS)
+ else
return DYN_SUCCESS;
- else
- {
- master->set_rx_mode();
- return DYN_COMM_ERROR;
- }
}
else
return error;
@@ -516,6 +456,7 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
master->rx_no_answer=0x00;
// enable transmission
master->set_tx_mode();
+ master->sync_bulk_address=0x0000;
// send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS)
master->set_rx_mode();
@@ -619,23 +560,12 @@ unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned ch
if(master!=0x00000000)
{
- if(master->version==DYN_VER2)
- {
- error=comm_is_send_done(master->comm_dev);
- if(error==COMM_BUSY)
- 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;
+ if(master->tx_done==0x00)
+ return DYN_BUSY;
+ else
+ return DYN_SUCCESS;
}
- else
+ else
return error;
}
@@ -664,6 +594,7 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
master->rx_no_answer=0x00;
// enable transmission
master->set_tx_mode();
+ master->sync_bulk_address=0x0000;
// send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS)
master->set_rx_mode();
@@ -746,28 +677,30 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
}
/* 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 */
- 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->hal_dev=hal_dev;
master->version=version;
- dev->data=master;
+ master->rx_dma_mode=0x00;
+ master->tx_done=0x01;
/* initialize the internal callbacks */
master->set_tx_mode=dummy_dyn_master_set_tx_mode;
master->set_rx_mode=dummy_dyn_master_set_rx_mode;
master->enable_power=dummy_dyn_master_enable_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 */
master->packet_ready=0x00;
- master->received_bytes=0x00;
master->rx_timeout_ms=50;
master->rx_no_answer=0x00;
master->rx_num_packets=0x00;
master->return_level=return_all;
- master->packet_bytes=0;
+ master->sync_bulk_address;
master->set_rx_mode();
}
@@ -1140,6 +1073,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
master->rx_no_answer=0x00;
// enable transmission
master->set_tx_mode();
+ master->sync_bulk_address=0x0000;
// send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS)
{
@@ -1206,6 +1140,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
master->rx_no_answer=0x00;
// enable transmission
master->set_tx_mode();
+ master->sync_bulk_address=0x0000;
// send the data
if((error=dyn_master_send(master))!=DYN_SUCCESS)
{
--
GitLab