From 21e9d81889da4bd246d8d64441af2022a1b396f5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 9 Mar 2014 22:23:52 +0000
Subject: [PATCH] Implemented the dynamixel interface with DMA. Solved some
 minor bugs.

---
 src/bioloid_stm32.c                           |  31 +--
 ...ter_uart.c => dynamixel_master_uart_dma.c} | 197 ++++++++++++------
 src/dynamixel_slave_spi.c                     |   7 -
 3 files changed, 145 insertions(+), 90 deletions(-)
 rename src/{dynamixel_master_uart.c => dynamixel_master_uart_dma.c} (64%)

diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index e9b0ebd..ba2364c 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -53,12 +53,22 @@ int32_t main(void)
   time_init();
   /* initialize the dynamixel master interface */
   dyn_master_init();  
-  dyn_master_set_timeout(10);
+  dyn_master_set_timeout(20);
   /* initialize the dynamixel slave interface*/
   dyn_slave_init();
   EE_ReadVariable(DEVICE_ID_OFFSET,&address);
   dyn_slave_set_address((uint8_t)address);
 
+  while(1)
+  {
+    if(dyn_master_ping(1))
+      GPIO_SetBits(GPIOD,GPIO_Pin_12);
+    else
+      GPIO_ResetBits(GPIOD,GPIO_Pin_12);
+    GPIO_ToggleBits(GPIOD,GPIO_Pin_14);
+    delay_ms(100);
+  }
+
   while(1)                             /* main function does not return */
   {
     if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
@@ -71,7 +81,6 @@ int32_t main(void)
         if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
         {
           // process the packet
-          GPIO_ToggleBits(GPIOD, GPIO_Pin_15); 
           switch(dyn_get_instruction(inst_packet))
           {
             case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
@@ -104,24 +113,6 @@ int32_t main(void)
         dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
       }
     }
-
-    // polling example
-//    GPIO_ToggleBits(GPIOD, GPIO_Pin_13);
-//    if(dyn_master_ping(4))
-//      GPIO_SetBits(GPIOD, GPIO_Pin_12);
-//    else
-//      GPIO_ResetBits(GPIOD, GPIO_Pin_12);
-//    delay_ms(10);
-//    if(dyn_master_ping(3))
-//      GPIO_SetBits(GPIOD, GPIO_Pin_14);
-//    else
-//      GPIO_ResetBits(GPIOD, GPIO_Pin_14);
-//    delay_ms(1000);
-//    while(!SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_RXNE));
-//    data=SPI_I2S_ReceiveData(SPIx);
-//    while(!SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_TXE));
-//    SPI_I2S_SendData(SPIx,data);
-//    GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
   }
 }
 
diff --git a/src/dynamixel_master_uart.c b/src/dynamixel_master_uart_dma.c
similarity index 64%
rename from src/dynamixel_master_uart.c
rename to src/dynamixel_master_uart_dma.c
index ee07286..83c88c7 100755
--- a/src/dynamixel_master_uart.c
+++ b/src/dynamixel_master_uart_dma.c
@@ -29,40 +29,60 @@
 #define     USART_RX_EN_GPIO_CLK     RCC_AHB1Periph_GPIOA
 #define     USART_RX_EN_SOURCE       GPIO_PinSource0
 
+/* DMA configuration */
+#define     USART_DR_ADDRESS         ((uint32_t)USART2 + 0x04) 
+#define     USART_DMA                DMA1
+#define     USART_DMA_CLK            RCC_AHB1Periph_DMA1
+
+#define     USART_TX_DMA_CHANNEL     DMA_Channel_4
+#define     USART_TX_DMA_STREAM      DMA1_Stream6
+#define     USART_TX_DMA_FLAG_FEIF   DMA_FLAG_FEIF6
+#define     USART_TX_DMA_FLAG_DMEIF  DMA_FLAG_DMEIF6
+#define     USART_TX_DMA_FLAG_TEIF   DMA_FLAG_TEIF6
+#define     USART_TX_DMA_FLAG_HTIF   DMA_FLAG_HTIF6
+#define     USART_TX_DMA_FLAG_TCIF   DMA_FLAG_TCIF6
+
+#define     USART_RX_DMA_CHANNEL     DMA_Channel_4
+#define     USART_RX_DMA_STREAM      DMA1_Stream5
+#define     USART_RX_DMA_FLAG_FEIF   DMA_FLAG_FEIF5
+#define     USART_RX_DMA_FLAG_DMEIF  DMA_FLAG_DMEIF5
+#define     USART_RX_DMA_FLAG_TEIF   DMA_FLAG_TEIF5
+#define     USART_RX_DMA_FLAG_HTIF   DMA_FLAG_HTIF5
+#define     USART_RX_DMA_FLAG_TCIF   DMA_FLAG_TCIF5
+
+#define     USART_DMA_TX_IRQn        DMA1_Stream6_IRQn
+#define     USART_DMA_RX_IRQn        DMA1_Stream5_IRQn
+#define     USART_DMA_TX_IRQHandler  DMA1_Stream6_IRQHandler
+#define     USART_DMA_RX_IRQHandler  DMA1_Stream5_IRQHandler
+
 #define     MAX_BUFFER_LEN           1024
 
 // private variables
 uint16_t dyn_master_timeout;// answer reception timeout
 // input buffer
 uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_master_rx_num_data;
+volatile uint8_t dyn_master_receiving_header;
 // output buffer
 uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_master_tx_num_data;
-uint16_t dyn_master_tx_ptr;
 // instruction packet ready flag
 volatile uint8_t dyn_master_packet_ready;
 // sending status packet flag
 volatile uint8_t dyn_master_sending_packet;
+// DMA initialization data structures
+DMA_InitTypeDef  DMA_TX_InitStructure;
+DMA_InitTypeDef  DMA_RX_InitStructure;
 
 // private functions
-void dyn_parse_status_packet(void)
-{
-  if(dyn_master_rx_num_data>3)// the length byte has been received
-  {
-    if(dyn_master_rx_num_data==(dyn_get_length(dyn_master_rx_buffer)+4))
-      dyn_master_packet_ready=0x01;
-  }  
-}
-
 void dyn_master_send(void)
 {
   // wait until any previous transmission ends
   while(dyn_master_sending_packet);
-  // send the first byte
-  dyn_master_tx_num_data=dyn_get_length(dyn_master_tx_buffer)+4;
+  // set the DMA transfer
+  DMA_SetCurrDataCounter(USART_TX_DMA_STREAM,dyn_get_length(dyn_master_tx_buffer)+4);
+  DMA_Cmd(USART_TX_DMA_STREAM,ENABLE);
+  USART_ClearFlag(USART,USART_FLAG_TC);
+  USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
   dyn_master_sending_packet=0x01;
-  USART_ITConfig(USART, USART_IT_TC, ENABLE);
 }
 
 uint8_t dyn_master_receive(void)
@@ -78,7 +98,6 @@ uint8_t dyn_master_receive(void)
       return DYN_TIMEOUT; 
   }
   dyn_master_packet_ready=0x00;
-  dyn_master_rx_num_data=0x00;
   // check the input packet checksum
   if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
     return dyn_get_status_error(dyn_master_rx_buffer);
@@ -168,34 +187,50 @@ uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *d
 }
 
 // interrupt handlers
-/**
-  * @brief  This function handles USART1 global interrupt request.
-  * @param  None
-  * @retval None
-  */
 void USART_IRQHandler(void)
 {
-  if(USART_GetITStatus(USART, USART_IT_RXNE) != RESET)
+  if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
   {
-    /* Read one byte from the receive data register */
-    dyn_master_rx_buffer[dyn_master_rx_num_data++] = USART_ReceiveData(USART2);
-    dyn_parse_status_packet();
+    USART_ClearFlag(USART,USART_FLAG_TC);
+    USART_ITConfig(USART, USART_IT_TC, DISABLE);
+    dyn_master_sending_packet=0x00;
+    // set up the DMA RX transaction
+    DMA_RX_InitStructure.DMA_BufferSize = 4;
+    DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
+    DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
+    DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
+    DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
+    USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+    dyn_master_receiving_header=0x01;
   }
+}
 
-  if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
+void USART_DMA_TX_IRQHandler(void)
+{
+  DMA_ClearFlag(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
+  DMA_ClearITPendingBit(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
+  USART_ITConfig(USART, USART_IT_TC, ENABLE);
+}
+
+void USART_DMA_RX_IRQHandler(void)
+{
+  if(dyn_master_receiving_header==0x01)
   {
-    if(dyn_master_tx_num_data==0x00)// there is no more data to be sent
-    {
-      dyn_master_tx_ptr=0x00;
-      dyn_master_sending_packet=0x00;
-      // disable interrupts
-      USART_ITConfig(USART, USART_IT_TC, DISABLE);
-    }
-    else// there is still data to be sent
-    {
-      dyn_master_tx_num_data--;
-      USART_SendData(USART,dyn_master_tx_buffer[dyn_master_tx_ptr++]);// send the next_byte
-    }
+    DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+    DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+    DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
+    DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
+    DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
+    DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
+    USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+    dyn_master_receiving_header=0x00;
+  }
+  else
+  {
+    DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+    DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+    DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,DISABLE);
+    dyn_master_packet_ready=0x01;
   }
 }
 
@@ -250,14 +285,12 @@ void dyn_master_init(void)
     dyn_master_rx_buffer[i]=0x00;
     dyn_master_tx_buffer[i]=0x00;
   }
-  dyn_master_rx_num_data=0x00;
-  dyn_master_tx_num_data=0x00;
-  dyn_master_tx_ptr=0x00;
   // initialize the flags
   dyn_master_packet_ready=0x00;
   dyn_master_sending_packet=0x00;
+  dyn_master_receiving_header=0x01;
 
-  USART_DeInit(USART2);
+  USART_DeInit(USART);
   USART_StructInit(&USART_InitStructure);
   // configure the serial port
   USART_InitStructure.USART_BaudRate = 1000000;
@@ -267,37 +300,75 @@ void dyn_master_init(void)
   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
   USART_Init(USART, &USART_InitStructure);
-
-  // configure the interrupts
   NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
-  USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
-  USART_ITConfig(USART, USART_IT_ORE, DISABLE);
-  USART_ITConfig(USART, USART_IT_TXE, DISABLE);
-  USART_ITConfig(USART, USART_IT_TC, ENABLE);
-  USART_ITConfig(USART, USART_IT_CTS, DISABLE);
-  USART_ITConfig(USART, USART_IT_LBD, DISABLE);
-  USART_ITConfig(USART, USART_IT_IDLE, DISABLE);
-  USART_ITConfig(USART, USART_IT_PE, DISABLE);
-  USART_ITConfig(USART, USART_IT_ERR, DISABLE);
-
-  /* Enable the USART1 */
+  USART_ITConfig(USART, USART_IT_RXNE | USART_IT_ORE | USART_IT_TXE | USART_IT_CTS | USART_IT_LBD | USART_IT_IDLE | USART_IT_PE | USART_IT_ERR | USART_IT_TC, DISABLE);
+
+  // configure the DMA channels
+  /* Configure TX DMA */
+  RCC_AHB1PeriphClockCmd(USART_DMA_CLK, ENABLE);
+  DMA_DeInit(USART_TX_DMA_STREAM);
+  DMA_TX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+  DMA_TX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
+  DMA_TX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+  DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+  DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+  DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
+  DMA_TX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
+  DMA_TX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+  DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+  DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+  DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
+  DMA_TX_InitStructure.DMA_Channel = USART_TX_DMA_CHANNEL;
+  DMA_TX_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+  DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_tx_buffer;
+  DMA_Init(USART_TX_DMA_STREAM,&DMA_TX_InitStructure);
+  /* initialize DMA interrupts */
+  NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+  DMA_ITConfig(USART_TX_DMA_STREAM,DMA_IT_TC,ENABLE);
+  DMA_ITConfig(USART_TX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
+
+  /* Configure RX DMA */
+  DMA_DeInit(USART_RX_DMA_STREAM);
+  DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
+  DMA_RX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+  DMA_RX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
+  DMA_RX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+  DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+  DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+  DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
+  DMA_RX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
+  DMA_RX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+  DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+  DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+  DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
+  DMA_RX_InitStructure.DMA_Channel = USART_RX_DMA_CHANNEL;
+  DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
+  DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
+  DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
+  /* initialize DMA interrupts */
+  NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+  DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC | DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
+  
+  /* Enable the USART2 */
   USART_Cmd(USART, ENABLE);
 }
 
 void dyn_master_flush(void)
 {
   // flush only the reception buffer to avoid interrupting a sync_write command
-  dyn_master_rx_num_data=0x00;
-//  dyn_master_tx_num_data=0x00;
-//  dyn_master_tx_ptr=0x00;
-  // initialize the flags
-  dyn_master_packet_ready=0x00;
-//  dyn_master_sending_packet=0x00;
 }
 
 void dyn_master_set_timeout(uint16_t timeout_ms)
@@ -339,7 +410,7 @@ uint8_t dyn_master_ping(uint8_t id)
     // send the data
     dyn_master_send();
     // wait for the transmission to end
-    while(dyn_master_sending_packet);
+    while (dyn_master_sending_packet==0x01);
     dyn_master_enable_rx();
     // wait for the replay within the given timeout
     error=dyn_master_receive();
diff --git a/src/dynamixel_slave_spi.c b/src/dynamixel_slave_spi.c
index fc05f85..ca551a3 100644
--- a/src/dynamixel_slave_spi.c
+++ b/src/dynamixel_slave_spi.c
@@ -142,11 +142,6 @@ void dyn_slave_init(void)
   RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK | SPI_MOSI_GPIO_CLK | SPI_NSS_GPIO_CLK, ENABLE);
 
   /* SPI GPIO Configuration --------------------------------------------------*/
-  /* GPIO Deinitialisation */
-  GPIO_DeInit(SPI_SCK_GPIO_PORT);
-  GPIO_DeInit(SPI_MISO_GPIO_PORT);
-  GPIO_DeInit(SPI_MOSI_GPIO_PORT);
-  GPIO_DeInit(SPI_NSS_GPIO_PORT);
 
   /* Connect SPI pins to AF5 */
   GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
@@ -187,8 +182,6 @@ void dyn_slave_init(void)
   SPI_InitStructure.SPI_CRCPolynomial = 7;
   SPI_InitStructure.SPI_Mode = SPI_Mode_Slave;
   SPI_Init(SPI, &SPI_InitStructure);
-  /* Configure the Priority Group to 1 bit */
-  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
 
   /* Configure the SPI interrupt priority */
   NVIC_InitStructure.NVIC_IRQChannel = SPI_IRQn;
-- 
GitLab