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