Commit 6e6aea46 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Initial commit of the IMU STM32 module software.

parent 65a93f73
#ifndef ADXL345_ACCEL_H
#define ADXL345_ACCEL_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
// public functions
void imu_accel_get_config(void);
void imu_accel_start(void);
void imu_accel_stop(void);
void imu_accel_get_data(void);
void imu_accel_set_config(void);
uint8_t imu_accel_detect(void);
void imu_accel_process_data(void);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _HMC5843_compass_h
#define _HMC5843_compass_h
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
uint8_t imu_compass_detect(void);
void imu_compass_get_config(void);
void imu_compass_start(void);
void imu_compass_stop(void);
void imu_compass_get_data(void);
void imu_compass_set_config(void);
void imu_compass_process_data(void);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _ITG3200_GYRO_H
#define _ITG3200_GYRO_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
// public functions
uint8_t imu_gyro_detect(void);
void imu_gyro_get_config(void);
void imu_gyro_start(void);
void imu_gyro_stop(void);
void imu_gyro_get_data(void);
void imu_gyro_set_config(void);
void imu_gyro_process_data(void);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _IMU_9DOF_DMA_H
#define _IMU_9DOF_DMA_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
// general IMU public functions
void imu_init(void);
void imu_start(void);
void imu_stop(void);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _IMU_INT_H
#define _IMU_INT_H
#ifdef __cplusplus
extern "C" {
#endif
#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
#define IMU_TIMER TIM4
#define IMU_TIMER_IRQn TIM4_IRQn
#define IMU_TIMER_IRQHandler TIM4_IRQHandler
#define IMU_TIMER_CLK RCC_APB1Periph_TIM4
typedef enum {I2C_OP_DONE,I2C_WR_OP,I2C_RD_OP_CMD,I2C_RD_OP_DATA} i2c_op_t;
typedef enum {IMU_GET_ACCEL,IMU_GET_GYRO,IMU_GET_COMPASS} imu_state_t;
// I2C private variables
extern volatile uint8_t i2c_error;
extern volatile i2c_op_t i2c_op;
extern uint8_t i2c_rd_addr;
extern uint8_t i2c_wr_addr;
extern uint8_t i2c_data_out[32];
// dma initialization structures;
extern DMA_InitTypeDef IMU_DMA_TX_InitStructure;
extern DMA_InitTypeDef IMU_DMA_RX_InitStructure;
// private functions
void imu_wait_op_done(void);
uint8_t imu_is_op_done(void);
#ifdef __cplusplus
}
#endif
#endif
#include "ADXL345_accel.h"
#include "imu_int.h"
#include "ram.h"
// accelerometer registers
#define ACCEL_THRESH_TAP 0x1D
#define ACCEL_OFSX 0x1E
#define ACCEL_OFSY 0x1F
#define ACCEL_OFSZ 0x20
#define ACCEL_DUR 0x21
#define ACCEL_LATENT 0x22
#define ACCEL_WINDOW 0x23
#define ACCEL_THRESH_ACT 0x24
#define ACCEL_THRESH_INACT 0x25
#define ACCEL_TIME_INACT 0x26
#define ACCEL_ACT_INACT_CTL 0x27
#define ACCEL_THRESH_FF 0x28
#define ACCEL_TIME_FF 0x29
#define ACCEL_TAP_AXES 0x2A
#define ACCEL_ACT_TAP_STATUS 0x2B
#define ACCEL_BW_RATE 0x2C
#define ACCEL_POWER_CTL 0x2D
#define ACCEL_INT_ENABLE 0x2E
#define ACCEL_INT_MAP 0x2F
#define ACCEL_INT_SOURCE 0x30
#define ACCEL_DATA_FORMAT 0x31
// accelerometer configuration data
#define ACCEL_RATE_3200HZ 0x0F
#define ACCEL_RATE_1600HZ 0x0E
#define ACCEL_RATE_800HZ 0x0D
#define ACCEL_RATE_400HZ 0x0C
#define ACCEL_RATE_200HZ 0x0B
#define ACCEL_RATE_100HZ 0x0A
#define ACCEL_RATE_50HZ 0x09
#define ACCEL_RATE_25HZ 0x08
#define ACCEL_RATE_12_5HZ 0x07
#define ACCEL_RATE_6_25HZ 0x06
#define ACCEL_ENABLE_LINK 0x20
#define ACCEL_ENABLE_AUTOSLEEP 0x10
#define ACCEL_ENABLE_MEASURE 0x08
#define ACCEL_ENABLE_SLEEP 0x04
#define ACCEL_SLEEP_8HZ 0x00
#define ACCEL_SLEEP_4HZ 0x01
#define ACCEL_SLEEP_2HZ 0x02
#define ACCEL_SLEEP_1HZ 0x03
#define ACCEL_ENABLE_FULL_RES 0x08
#define ACCEL_JUSTIFY_LEFT 0x04
#define ACCEL_JUSTIFY_RIGHT 0x00
#define ACCEL_RANGE_2G 0x00
#define ACCEL_RANGE_4G 0x01
#define ACCEL_RANGE_8G 0x02
#define ACCEL_RANGE_16G 0x03
#define ACCEL_FIFO_BYPASS 0x00
#define ACCEL_FIFO_BUFFER 0x40
#define ACCEL_FIFO_STREAM 0x80
#define ACCEL_FIFO_TRIGGER 0xC0
// private variables
// 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=0x15;
const uint8_t accel_data_reg=0x32;
const uint8_t accel_data_len=0x06;
const uint8_t accel_fifo_reg=0x38;
const uint8_t accel_fifo_len=0x01;
const uint8_t accel_status_reg=0x39;
const uint8_t accel_status_len=0x01;
const uint8_t accel_rd_addr=0xA6;
const uint8_t accel_wr_addr=0xA7;
uint8_t i2c_accel_conf[21];
uint8_t i2c_accel_fifo_conf;
uint8_t i2c_accel_data[6];
uint8_t i2c_accel_status;
// public accelerometer functions
void imu_accel_get_config(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_conf_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_accel_conf;
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)&accel_conf_reg;
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_accel_start(void)
{
// enter measure mode
i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP]|=ACCEL_ENABLE_MEASURE;
i2c_data_out[0]=ACCEL_POWER_CTL;
i2c_data_out[1]=i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP];
// write the fifo configuration
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=2;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_accel_stop(void)
{
// enter measure mode
i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP]&=~ACCEL_ENABLE_MEASURE;
i2c_data_out[0]=ACCEL_POWER_CTL;
i2c_data_out[1]=i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP];
// write the fifo configuration
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=2;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_accel_get_data(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_data_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_accel_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)&accel_data_reg;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
}
void imu_accel_set_config(void)
{
uint8_t i;
// set the configuration
i2c_accel_conf[ACCEL_BW_RATE-ACCEL_THRESH_TAP]|=ACCEL_RATE_100HZ;
i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]&=0xF0;
i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]|=ACCEL_RANGE_8G;// +/-8g range
i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]|=ACCEL_JUSTIFY_RIGHT;// justify right
i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]|=ACCEL_ENABLE_FULL_RES;// enable full resolution according to the range
// write the configuration
for(i=0;i<accel_conf_len;i++)
i2c_data_out[i+1]=i2c_accel_conf[i];
i2c_data_out[0]=accel_conf_reg;
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=accel_conf_len+1;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
// set the fifo configuration
i2c_accel_fifo_conf=ACCEL_FIFO_STREAM;
i2c_data_out[0]=accel_fifo_reg;
i2c_data_out[1]=i2c_accel_fifo_conf;
// write the fifo configuration
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=accel_fifo_len+1;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
uint8_t imu_accel_detect(void)
{
i2c_accel_data[0]=0x00;
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_id_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_accel_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)&accel_id_reg;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=accel_rd_addr;
i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
if(i2c_accel_data[0]==accel_id)
{
imu_accel_get_config();
imu_accel_set_config();
return 0x01;
}
else
return 0x00;
}
void imu_accel_process_data(void)
{
uint8_t i;
int16_t value;
for(i=0;i<3;i++)
{
value=((int16_t)(i2c_accel_data[i*2]+(i2c_accel_data[i*2+1]<<8)))<<2;
ram_data[BIOLOID_IMU_ACCEL_X_L+i*2]=value&0xFF;
ram_data[BIOLOID_IMU_ACCEL_X_H+i*2]=(value>>8);
}
}
#include "HMC5843_compass.h"
#include "imu_int.h"
#include "ram.h"
// compass registers
#define COMPASS_CONF_A 0x00
#define COMPASS_CONF_B 0x01
#define COMPASS_MODE 0x02
#define COMPASS_STATUS 0x09
#define COMPASS_IDA 0x0A
#define COMPASS_IDB 0x0B
#define COMPASS_IDC 0x0C
// configuration values
#define COMPASS_DATA_RATE_0_5 0x00
#define COMPASS_DATA_RATE_1 0x04
#define COMPASS_DATA_RATE_2 0x08
#define COMPASS_DATA_RATE_5 0x0C
#define COMPASS_DATA_RATE_10 0x10
#define COMPASS_DATA_RATE_20 0x14
#define COMPASS_DATA_RATE_50 0x18
#define COMPASS_NO_BIAS 0x00
#define COMPASS_POS_BIAS 0x01
#define COMPASS_NEG_BIAS 0x02
#define COMPASS_FS_0_7 0x00
#define COMPASS_FS_1 0x20
#define COMPASS_FS_1_5 0x40
#define COMPASS_FS_2 0x60
#define COMPASS_FS_3_2 0x80
#define COMPASS_FS_3_8 0xA0
#define COMPASS_FS_4_5 0xC0
#define COMPASS_FS_6_5 0xE0
#define COMPASS_CONT_MODE 0x00
#define COMPASS_SINGLE_MODE 0x01
#define COMPASS_IDLE_MODE 0x02
#define COMPASS_SLEEP_MODE 0x03
// compass data adn configuration
const uint8_t compass_id_reg=0x0A;
const uint8_t compass_id[3]={'H','4','3'};
const uint8_t compass_id_len=0x03;
const uint8_t compass_conf_reg=0x00;
const uint8_t compass_conf_len=0x03;
const uint8_t compass_data_reg=0x03;
const uint8_t compass_data_len=0x06;
const uint8_t compass_status_reg=0x09;
const uint8_t compass_status_len=0x01;
const uint8_t compass_rd_addr=0x3D;
const uint8_t compass_wr_addr=0x3C;
uint8_t i2c_compass_conf[3];
uint8_t i2c_compass_data[6];
// compass public functions
uint8_t imu_compass_detect(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=compass_id_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_compass_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)&compass_id_reg;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_compass_data[0]=0x00;
i2c_compass_data[1]=0x00;
i2c_compass_data[2]=0x00;
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
if(i2c_compass_data[0]==compass_id[0] && i2c_compass_data[1]==compass_id[1] && i2c_compass_data[2]==compass_id[2])
{
imu_compass_get_config();
imu_compass_set_config();
return 0x01;
}
else
return 0x00;
}
void imu_compass_get_config(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=compass_conf_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_compass_conf;
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)&compass_conf_reg;
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_compass_start(void)
{
// enter measure mode
i2c_compass_conf[COMPASS_MODE-COMPASS_CONF_A]=COMPASS_CONT_MODE;
i2c_data_out[0]=COMPASS_MODE;
i2c_data_out[1]=i2c_compass_conf[COMPASS_MODE-COMPASS_CONF_A];
// write the fifo configuration
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=2;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_compass_stop(void)
{
// enter measure mode
i2c_compass_conf[COMPASS_MODE-COMPASS_CONF_A]=COMPASS_SLEEP_MODE;
i2c_data_out[0]=COMPASS_MODE;
i2c_data_out[1]=i2c_compass_conf[COMPASS_MODE-COMPASS_CONF_A];
// write the fifo configuration
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=2;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_compass_get_data(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=compass_data_len;
IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_compass_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)&compass_data_reg;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
}
void imu_compass_set_config(void)
{
uint8_t i;
// set the configuration
i2c_compass_conf[COMPASS_CONF_A-COMPASS_CONF_A]=COMPASS_DATA_RATE_50 | COMPASS_NO_BIAS;
i2c_compass_conf[COMPASS_CONF_B-COMPASS_CONF_A]=COMPASS_FS_1;
i2c_compass_conf[COMPASS_MODE-COMPASS_CONF_A]=COMPASS_SLEEP_MODE;
// write the configuration
for(i=0;i<compass_conf_len;i++)
i2c_data_out[i+1]=i2c_compass_conf[i];
i2c_data_out[0]=compass_conf_reg;
i2c_op=I2C_WR_OP;
IMU_DMA_TX_InitStructure.DMA_BufferSize=compass_conf_len+1;
IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
}
void imu_compass_process_data(void)
{
uint8_t i;
int16_t value;
for(i=0;i<3;i++)
{
value=(((int16_t)(i2c_compass_data[i*2+1]+(i2c_compass_data[i*2]<<8)))<<13)/1300;
ram_data[BIOLOID_IMU_COMPASS_X_L+i*2]=value&0xFF;
ram_data[BIOLOID_IMU_COMPASS_X_H+i*2]=(value>>8);
}
}
#include "ITG3200_gyro.h"
#include "imu_int.h"
#include "ram.h"
// gyroscope registers
#define GYRO_SMPL_DIV 0x15
#define GYRO_CONFIG 0x16
#define GYRO_INT_CONFIG 0x17
#define GYRO_STATUS 0x1A
#define GYRO_POWER_MGM 0x3E
// configuration data
#define GYRO_FS_2000 0x18
#define GYRO_BW_256 0x00
#define GYRO_BW_188 0x01
#define GYRO_BW_98 0x02
#define GYRO_BW_42 0x03
#define GYRO_BW_20 0x04
#define GYRO_BW_10 0x05
#define GYRO_BW_5 0x06
#define GYRO_INT_ACTIVE_LOW 0x80
#define GYRO_INT_ACTIVE_HIGH 0x00
#define GYRO_INT_OPEN_DRAIN 0x40
#define GYRO_INT_PUSH_PULL 0x00
#define GYRO_INT_LATCH 0x20
#define GYRO_INT_PULSE 0x00
#define GYRO_INT_CLEAR_RD_ANY 0x10
#define GYRO_INT_CLEAR_RD_STATUS 0x00
#define GYRO_INT_DEVICE_READY 0x04
#define GYRO_INT_DATA_READY 0x01
#define GYRO_INT_DEVICE_READY_FLAG 0x04
#define GYRO_INT_DATA_READY_FLAG 0x01
#define GYRO_RESET 0x80
#define GYRO_SLEEP_MODE 0x40
#define GYRO_STBY_X 0x20
#define GYRO_STBY_Y 0x10
#define GYRO_STBY_Z 0x08
#define GYRO_CLK_INT 0x00
#define GYRO_CLK_PLL_X 0x01
#define GYRO_CLK_PLL_Y 0x02
#define GYRO_CLK_PLL_Z 0x03
#define GYRO_CLK_PLL_32768 0x04
#define GYRO_CLK_PLL_19_2M 0x05
// gyroscope data and configuration
const uint8_t gyro_id_reg=0x00;
const uint8_t gyro_id=0x68;
const uint8_t gyro_id_len=0x01;
const uint8_t gyro_conf_reg=0x15;
const uint8_t gyro_conf_len=0x03;
const uint8_t gyro_status_reg=0x1A;
const uint8_t gyro_status_len=0x01;
const uint8_t gyro_data_reg=0x1B;