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

Added an IMU module using DMA. 

Only the accelerometer paret is partially implemented.
parent 5e1b0331
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/dynamixel_master_uart.h
+ 2
− 2
View file @ 3bee016b
#ifndef _DYNAMIXEL_MASTER_H #ifndef _DYNAMIXEL_MASTER_DMA_H
#define _DYNAMIXEL_MASTER_H #define _DYNAMIXEL_MASTER_DMA_H
#include "stm32f4xx.h" #include "stm32f4xx.h"
#include "dynamixel.h" #include "dynamixel.h"
......
include/imu_9dof_dma.h 0 → 100644
+ 49
− 0
View file @ 3bee016b
#ifndef _IMU_9DOF_DMA_H
#define _IMU_9DOF_DMA_H
#include "stm32f4xx.h"
// accelerometer configuration data
#define RATE_3200HZ 0x0F
#define RATE_1600HZ 0x0E
#define RATE_800HZ 0x0D
#define RATE_400HZ 0x0C
#define RATE_200HZ 0x0B
#define RATE_100HZ 0x0A
#define RATE_50HZ 0x09
#define RATE_25HZ 0x08
#define RATE_12_5HZ 0x07
#define RATE_6_25HZ 0x06
#define ENABLE_LINK 0x20
#define ENABLE_AUTOSLEEP 0x10
#define ENABLE_MEASURE 0x08
#define ENABLE_SLEEP 0x04
#define SLEEP_8HZ 0x00
#define SLEEP_4HZ 0x01
#define SLEEP_2HZ 0x02
#define SLEEP_1HZ 0x03
#define ENABLE_FUL_RES 0x08
#define JUSTIFY_LEFT 0x04
#define JUSTIFY_RIGHT 0x00
#define RANGE_2G 0x00
#define RANGE_4G 0x01
#define RANGE_8G 0x02
#define RANGE_16G 0x03
// general IMU public functions
void imu_init(void);
// Accelerometer public functions
uint8_t imu_accel_detect(void);
void imu_accel_set_offset(uint8_t off_x, uint8_t off_y, uint8_t off_z);
void imu_accel_set_data_rate(uint8_t rate_hz);
void imu_accel_set_measure_mode(void);
void imu_accel_set_sleep_mode(void);
void imu_accel_set_sleep_rate(uint8_t rate_hz);
void imu_accel_set_range(uint8_t range);
void imu_accel_justify_left(void);
void imu_accel_justify_right(void);
void imu_accel_config(void);
#endif
src/bioloid_stm32.c
+ 20
− 12
View file @ 3bee016b
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
#include "dynamixel_master_uart.h" #include "dynamixel_master_uart.h"
#include "dynamixel_slave_spi.h" #include "dynamixel_slave_spi.h"
#include "eeprom.h" #include "eeprom.h"
#include "imu_9dof_dma.h"
uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data) uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
{ {
...@@ -48,25 +49,32 @@ int32_t main(void) ...@@ -48,25 +49,32 @@ int32_t main(void)
GPIO_Init(GPIOD, &GPIO_InitStructure); GPIO_Init(GPIOD, &GPIO_InitStructure);
/* initialize EEPROM */ /* initialize EEPROM */
EE_Init(); // EE_Init();
/* initialize the 1ms system timer */ /* initialize the 1ms system timer */
time_init(); time_init();
/* initialize the dynamixel master interface */ /* initialize the dynamixel master interface */
dyn_master_init(); // dyn_master_init();
dyn_master_set_timeout(20); // dyn_master_set_timeout(20);
/* initialize the dynamixel slave interface*/ /* initialize the dynamixel slave interface*/
dyn_slave_init(); // dyn_slave_init();
EE_ReadVariable(DEVICE_ID_OFFSET,&address); // EE_ReadVariable(DEVICE_ID_OFFSET,&address);
dyn_slave_set_address((uint8_t)address); // dyn_slave_set_address((uint8_t)address);
imu_init();
if(imu_detect_accel())
GPIO_SetBits(GPIOD,GPIO_Pin_12);
else
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
while(1) while(1)
{ {
if(dyn_master_ping(1)) imu_config_accel();
GPIO_SetBits(GPIOD,GPIO_Pin_12); // if(dyn_master_ping(1))
else // GPIO_SetBits(GPIOD,GPIO_Pin_12);
GPIO_ResetBits(GPIOD,GPIO_Pin_12); // else
GPIO_ToggleBits(GPIOD,GPIO_Pin_14); // GPIO_ResetBits(GPIOD,GPIO_Pin_12);
delay_ms(100); GPIO_ToggleBits(GPIOD,GPIO_Pin_15);
delay_ms(1000);
} }
while(1) /* main function does not return */ while(1) /* main function does not return */
......
src/imu_9dof_dma.c 0 → 100644
+ 345
− 0
View file @ 3bee016b
#include "imu_9dof_dma.h"
#define I2C_IMU_DMA DMA1
#define I2C_IMU_DMA_CHANNEL DMA_Channel_3
#define I2C_IMU_DR_ADDRESS ((uint32_t)I2C3+0x10)
#define I2C_IMU_DMA_STREAM_TX DMA1_Stream4
#define I2C_IMU_DMA_STREAM_RX DMA1_Stream2
#define I2C_IMU_TX_DMA_TCFLAG DMA_FLAG_TCIF4
#define I2C_IMU_TX_DMA_FEIFLAG DMA_FLAG_FEIF4
#define I2C_IMU_TX_DMA_DMEIFLAG DMA_FLAG_DMEIF4
#define I2C_IMU_TX_DMA_TEIFLAG DMA_FLAG_TEIF4
#define I2C_IMU_TX_DMA_HTIFLAG DMA_FLAG_HTIF4
#define I2C_IMU_RX_DMA_TCFLAG DMA_FLAG_TCIF2
#define I2C_IMU_RX_DMA_FEIFLAG DMA_FLAG_FEIF2
#define I2C_IMU_RX_DMA_DMEIFLAG DMA_FLAG_DMEIF2
#define I2C_IMU_RX_DMA_TEIFLAG DMA_FLAG_TEIF2
#define I2C_IMU_RX_DMA_HTIFLAG DMA_FLAG_HTIF2
#define DMA_IMU_CLK RCC_AHB1Periph_DMA1
#define I2C_IMU_TX_DMA_IRQn DMA1_Stream4_IRQn
#define I2C_IMU_RX_DMA_IRQn DMA1_Stream2_IRQn
#define I2C_IMU_TX_DMA_IRQHandler DMA1_Stream4_IRQHandler
#define I2C_IMU_RX_DMA_IRQHandler DMA1_Stream2_IRQHandler
#define I2C_IMU I2C3
#define I2C_IMU_CLK RCC_APB1Periph_I2C3
#define I2C_IMU_SDA_GPIO_CLK RCC_AHB1Periph_GPIOC
#define I2C_IMU_SDA_PIN GPIO_Pin_9
#define I2C_IMU_SDA_GPIO_PORT GPIOC
#define I2C_IMU_SDA_SOURCE GPIO_PinSource9
#define I2C_IMU_SDA_AF GPIO_AF_I2C3
#define I2C_IMU_SCL_GPIO_CLK RCC_AHB1Periph_GPIOA
#define I2C_IMU_SCL_PIN GPIO_Pin_8
#define I2C_IMU_SCL_GPIO_PORT GPIOA
#define I2C_IMU_SCL_SOURCE GPIO_PinSource8
#define I2C_IMU_SCL_AF GPIO_AF_I2C3
#define I2C_IMU_EV_IRQn I2C3_EV_IRQn
#define I2C_IMU_ER_IRQn I2C3_ER_IRQn
#define I2C_IMU_EV_IRQHandler I2C3_EV_IRQHandler
#define I2C_IMU_ER_IRQHandler I2C3_ER_IRQHandler
typedef enum {I2C_OP_DONE,I2C_WR_OP,I2C_RD_OP_CMD,I2C_RD_OP_DATA} i2c_op_t;
// private variables
// I2C private variables
volatile uint8_t i2c_error;
volatile i2c_op_t i2c_op;
uint8_t i2c_conf[32];
uint8_t i2c_data[32];
// accelerometer data and configuration
const uint8_t accel_id_reg=0x00;
const uint8_t accel_id=0xE5;
const uint8_t accel_id_len=0x01;
const uint8_t accel_conf_reg=0x1D;
const uint8_t accel_conf_len=0x16;
const uint8_t accel_data_reg=0x32;
const uint8_t accel_data_len=0x06;
const uint8_t accel_rd_addr=0xA6;
const uint8_t accel_wr_addr=0xA7;
// dma initialization structures;
DMA_InitTypeDef IMU_DMA_TX_InitStructure;
DMA_InitTypeDef IMU_DMA_RX_InitStructure;
// interrupt handlers
void I2C_IMU_EV_IRQHandler(void)
{
switch (I2C_GetLastEvent(I2C_IMU))
{
/* Test on I2C1 EV5 and clear it */
case I2C_EVENT_MASTER_MODE_SELECT:
if(i2c_op==I2C_RD_OP_DATA)
I2C_Send7bitAddress(I2C_IMU,accel_rd_addr,I2C_Direction_Receiver);
else
I2C_Send7bitAddress(I2C_IMU,accel_wr_addr,I2C_Direction_Transmitter);
break;
/* Test on I2C1 EV6 and first EV8 and clear them */
case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED:
I2C_ITConfig(I2C_IMU, I2C_IT_EVT, DISABLE);
/* enable DMA transfer */
I2C_DMACmd(I2C_IMU, ENABLE);
DMA_Cmd(I2C_IMU_DMA_STREAM_TX, ENABLE);
break;
case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED:
I2C_ITConfig(I2C_IMU, I2C_IT_EVT, DISABLE);
I2C_AcknowledgeConfig(I2C_IMU,DISABLE);
/* enable DMA transfer */
I2C_DMACmd(I2C_IMU, ENABLE);
DMA_Cmd(I2C_IMU_DMA_STREAM_RX, ENABLE);
break;
default:
break;
}
}
void I2C_IMU_ER_IRQHandler(void)
{
if(I2C_GetFlagStatus(I2C_IMU,I2C_FLAG_AF)!=RESET)
{
i2c_error=0x01;
I2C_ClearFlag(I2C_IMU,I2C_FLAG_AF);
if(i2c_op==I2C_RD_OP_DATA)
DMA_Cmd(I2C_IMU_DMA_STREAM_RX, DISABLE);
else
DMA_Cmd(I2C_IMU_DMA_STREAM_TX, DISABLE);
I2C_DMACmd(I2C_IMU, DISABLE);
I2C_GenerateSTOP(I2C_IMU, ENABLE);
i2c_op=I2C_OP_DONE;
}
I2C_ClearITPendingBit(I2C_IMU,I2C_IT_ERR);
}
void I2C_IMU_TX_DMA_IRQHandler(void)
{
DMA_ClearFlag(I2C_IMU_DMA_STREAM_TX,I2C_IMU_TX_DMA_TCFLAG);
DMA_ClearITPendingBit(I2C_IMU_DMA_STREAM_TX,I2C_IMU_TX_DMA_TCFLAG);
I2C_ITConfig(I2C_IMU, I2C_IT_EVT, ENABLE);
if(i2c_op==I2C_RD_OP_CMD)
{
I2C_GenerateSTART(I2C_IMU, ENABLE);
i2c_op=I2C_RD_OP_DATA;
}
else
{
I2C_GenerateSTOP(I2C_IMU, ENABLE);
i2c_op=I2C_OP_DONE;
}
}
void I2C_IMU_RX_DMA_IRQHandler(void)
{
DMA_ClearFlag(I2C_IMU_DMA_STREAM_RX,I2C_IMU_RX_DMA_TCFLAG);
DMA_ClearITPendingBit(I2C_IMU_DMA_STREAM_RX,I2C_IMU_RX_DMA_TCFLAG);
I2C_ITConfig(I2C_IMU, I2C_IT_EVT, ENABLE);
I2C_GenerateSTOP(I2C_IMU, ENABLE);
i2c_op=I2C_OP_DONE;
}
// private functions
void imu_wait_op_done(void)
{
while(i2c_op!=I2C_OP_DONE);
}
uint8_t imu_is_op_done(void)
{
if(i2c_op==I2C_OP_DONE)
return 0x01;
else
return 0x00;
}
uint8_t imu_accel_detect(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_id_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data;
DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_conf[0]=accel_id_reg;
i2c_data[0]=0x00;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
if(i2c_data[0]==accel_id)
return 0x01;
else
return 0x00;
}
void imu_accel_set_offset(uint8_t off_x, uint8_t off_y, uint8_t off_z)
{
}
void imu_accel_set_data_rate(uint8_t rate_hz)
{
}
void imu_accel_set_measure_mode(void)
{
}
void imu_accel_set_sleep_mode(void)
{
}
void imu_accel_set_sleep_rate(uint8_t rate_hz)
{
}
void imu_accel_set_range(uint8_t range)
{
}
void imu_accel_justify_left(void)
{
}
void imu_accel_justify_right(void)
{
}
void imu_accel_config(void)
{
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=accel_conf_len+1;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_conf[0]=accel_conf_reg;
i2c_conf[15]=0x0A;
i2c_conf[20]=0x02;
I2C_GenerateSTART(I2C_IMU, ENABLE);
}
// public functions
void imu_init(void)
{
uint8_t i;
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
/* RCC Configuration */
/*I2C Peripheral clock enable */
RCC_APB1PeriphClockCmd(I2C_IMU_CLK, ENABLE);
/*SDA GPIO clock enable */
RCC_AHB1PeriphClockCmd(I2C_IMU_SDA_GPIO_CLK, ENABLE);
/*SCL GPIO clock enable */
RCC_AHB1PeriphClockCmd(I2C_IMU_SCL_GPIO_CLK, ENABLE);
/* Enable the DMA clock */
RCC_AHB1PeriphClockCmd(DMA_IMU_CLK, ENABLE);
/* GPIO Configuration */
/*Configure I2C SCL pin */
GPIO_InitStructure.GPIO_Pin = I2C_IMU_SCL_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(I2C_IMU_SCL_GPIO_PORT, &GPIO_InitStructure);
/*Configure I2C SDA pin */
GPIO_InitStructure.GPIO_Pin = I2C_IMU_SDA_PIN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(I2C_IMU_SDA_GPIO_PORT, &GPIO_InitStructure);
/* Connect PXx to I2C_SCL */
GPIO_PinAFConfig(I2C_IMU_SCL_GPIO_PORT, I2C_IMU_SCL_SOURCE, I2C_IMU_SCL_AF);
/* Connect PXx to I2C_SDA */
GPIO_PinAFConfig(I2C_IMU_SDA_GPIO_PORT, I2C_IMU_SDA_SOURCE, I2C_IMU_SDA_AF);
/* I2C Struct Initialize */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0x00;
I2C_InitStructure.I2C_Ack = I2C_Ack_Disable;
I2C_InitStructure.I2C_ClockSpeed = 400000;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_Init(I2C_IMU, &I2C_InitStructure);
/* configure I2C interrupts */
NVIC_InitStructure.NVIC_IRQChannel = I2C_IMU_ER_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = I2C_IMU_EV_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
I2C_ITConfig(I2C_IMU, I2C_IT_ERR | I2C_IT_EVT, ENABLE);
// enable I2C1
I2C_Cmd(I2C_IMU, ENABLE);
// initialize data
i2c_error=0x00;
i2c_op=I2C_OP_DONE;
for(i=0;i<32;i++)
{
i2c_data[i]=0x00;
i2c_conf[i]=0x00;
}
/* configure DMA TX */
DMA_DeInit(I2C_IMU_DMA_STREAM_TX);
IMU_DMA_TX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
IMU_DMA_TX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
IMU_DMA_TX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
IMU_DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
IMU_DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
IMU_DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
IMU_DMA_TX_InitStructure.DMA_PeripheralBaseAddr = I2C_IMU_DR_ADDRESS;
IMU_DMA_TX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
IMU_DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
IMU_DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
IMU_DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
IMU_DMA_TX_InitStructure.DMA_Channel = I2C_IMU_DMA_CHANNEL;
IMU_DMA_TX_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = I2C_IMU_TX_DMA_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(I2C_IMU_DMA_STREAM_TX,DMA_IT_TC,ENABLE);
DMA_ITConfig(I2C_IMU_DMA_STREAM_TX,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
/* configure DMA Rx */
DMA_DeInit(I2C_IMU_DMA_STREAM_RX);
IMU_DMA_RX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
IMU_DMA_RX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
IMU_DMA_RX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
IMU_DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
IMU_DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
IMU_DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
IMU_DMA_RX_InitStructure.DMA_PeripheralBaseAddr = I2C_IMU_DR_ADDRESS;
IMU_DMA_RX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
IMU_DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
IMU_DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
IMU_DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
IMU_DMA_RX_InitStructure.DMA_Channel = I2C_IMU_DMA_CHANNEL;
IMU_DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = I2C_IMU_RX_DMA_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(I2C_IMU_DMA_STREAM_RX,DMA_IT_TC,ENABLE);
DMA_ITConfig(I2C_IMU_DMA_STREAM_RX,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
}
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