diff --git a/include/ADXL345_accel.h b/include/ADXL345_accel.h
deleted file mode 100644
index d9a2ba199fd8d45fba6dd8e11afb549cc73190b4..0000000000000000000000000000000000000000
--- a/include/ADXL345_accel.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#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
diff --git a/include/HMC5843_compass.h b/include/HMC5843_compass.h
deleted file mode 100644
index 2a24978ccb6f059e18b083a1a9359fdcc6648025..0000000000000000000000000000000000000000
--- a/include/HMC5843_compass.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#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
diff --git a/include/ITG3200_gyro.h b/include/ITG3200_gyro.h
deleted file mode 100644
index 6540e77a62fb325aa86933971f586ba26721b6ed..0000000000000000000000000000000000000000
--- a/include/ITG3200_gyro.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#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
diff --git a/include/imu_9dof_dma.h b/include/imu_9dof_dma.h
deleted file mode 100644
index 6b22fb67a6128c2d4c1f84143a30b21328393190..0000000000000000000000000000000000000000
--- a/include/imu_9dof_dma.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#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
diff --git a/include/imu_int.h b/include/imu_int.h
deleted file mode 100644
index 32b77838eac40835c12477d7249cd1dc3478b8dd..0000000000000000000000000000000000000000
--- a/include/imu_int.h
+++ /dev/null
@@ -1,75 +0,0 @@
-#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
diff --git a/src/ADXL345_accel.c b/src/ADXL345_accel.c
deleted file mode 100644
index 1251cb34ddc635a46a4ddf88976b078338de7af2..0000000000000000000000000000000000000000
--- a/src/ADXL345_accel.c
+++ /dev/null
@@ -1,220 +0,0 @@
-#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);
- }
-}
-
diff --git a/src/HMC5843_compass.c b/src/HMC5843_compass.c
deleted file mode 100644
index fbda706a87462ecce13c4bf0ef5e35f7fe3671b9..0000000000000000000000000000000000000000
--- a/src/HMC5843_compass.c
+++ /dev/null
@@ -1,181 +0,0 @@
-#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);
- }
-}
-
-
diff --git a/src/ITG3200_gyro.c b/src/ITG3200_gyro.c
deleted file mode 100644
index 241c3ae6f291d6a46fb92e140fd565b485fb45dd..0000000000000000000000000000000000000000
--- a/src/ITG3200_gyro.c
+++ /dev/null
@@ -1,219 +0,0 @@
-#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;
-const uint8_t gyro_data_len=0x08;
-const uint8_t gyro_power_reg=0x3E;
-const uint8_t gyro_power_len=0x01;
-const uint8_t gyro_rd_addr=0xD0;
-const uint8_t gyro_wr_addr=0xD1;
-uint8_t i2c_gyro_conf[3];
-uint8_t i2c_gyro_data[8];
-uint8_t i2c_gyro_power_mgm;
-uint8_t i2c_gyro_status;
-
-// gyroscope private functions
-uint8_t imu_gyro_detect(void)
-{
- i2c_op=I2C_RD_OP_CMD;
- IMU_DMA_RX_InitStructure.DMA_BufferSize=gyro_id_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_gyro_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)&gyro_id_reg;
- DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
- i2c_gyro_data[0]=0x00;
- i2c_rd_addr=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
-
- imu_wait_op_done();
- if((i2c_gyro_data[0]&0x7E)==gyro_id)
- {
- imu_gyro_get_config();
- imu_gyro_set_config();
- return 0x01;
- }
- else
- return 0x00;
-}
-
-void imu_gyro_get_config(void)
-{
- /* read the configuration */
- i2c_op=I2C_RD_OP_CMD;
- IMU_DMA_RX_InitStructure.DMA_BufferSize=gyro_conf_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_gyro_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)&gyro_conf_reg;
- i2c_rd_addr=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
- imu_wait_op_done();
-
- /* read trhe power management */
- i2c_op=I2C_RD_OP_CMD;
- IMU_DMA_RX_InitStructure.DMA_BufferSize=gyro_power_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_gyro_power_mgm;
- 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)&gyro_power_reg;
- i2c_rd_addr=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
- imu_wait_op_done();
-}
-
-void imu_gyro_start(void)
-{
- // enter measure mode
- i2c_gyro_power_mgm&=~(GYRO_SLEEP_MODE | GYRO_STBY_X | GYRO_STBY_Y | GYRO_STBY_Z);
- i2c_data_out[0]=GYRO_POWER_MGM;
- i2c_data_out[1]=i2c_gyro_power_mgm;
- // 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=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
- imu_wait_op_done();
-}
-
-void imu_gyro_stop(void)
-{
- // enter measure mode
- i2c_gyro_power_mgm|=(GYRO_SLEEP_MODE | GYRO_STBY_X | GYRO_STBY_Y | GYRO_STBY_Z);
- i2c_data_out[0]=GYRO_POWER_MGM;
- i2c_data_out[1]=i2c_gyro_power_mgm;
- // 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=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
- imu_wait_op_done();
-}
-
-void imu_gyro_get_data(void)
-{
- i2c_op=I2C_RD_OP_CMD;
- IMU_DMA_RX_InitStructure.DMA_BufferSize=gyro_data_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_gyro_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)&gyro_data_reg;
- DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
- i2c_rd_addr=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
-}
-
-void imu_gyro_set_config(void)
-{
- uint8_t i;
-
- // set the configuration
- i2c_gyro_conf[GYRO_SMPL_DIV-GYRO_SMPL_DIV]=9;// 100 Hz
- i2c_gyro_conf[GYRO_CONFIG-GYRO_SMPL_DIV]=GYRO_FS_2000 | GYRO_BW_188;
- i2c_gyro_conf[GYRO_INT_CONFIG-GYRO_SMPL_DIV]=0x00;// no interrupts enabled
- // write the configuration
- for(i=0;i<gyro_conf_len;i++)
- i2c_data_out[i+1]=i2c_gyro_conf[i];
- i2c_data_out[0]=gyro_conf_reg;
- i2c_op=I2C_WR_OP;
- IMU_DMA_TX_InitStructure.DMA_BufferSize=gyro_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=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
- imu_wait_op_done();
-
- // set the fifo configuration
- i2c_gyro_power_mgm|=GYRO_CLK_INT;
- i2c_data_out[0]=gyro_power_reg;
- i2c_data_out[1]=i2c_gyro_power_mgm;
- // write the fifo configuration
- i2c_op=I2C_WR_OP;
- IMU_DMA_TX_InitStructure.DMA_BufferSize=gyro_power_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=gyro_rd_addr;
- i2c_wr_addr=gyro_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
- imu_wait_op_done();
-}
-
-void imu_gyro_process_data(void)
-{
- uint8_t i;
- int16_t value;
-
- // process temperature
- ram_data[BIOLOID_IMU_TEMP]=((((int16_t)(i2c_gyro_data[1]+(i2c_gyro_data[0]<<8)))-13200)/280)+35;
- // process gyro data
- for(i=0;i<3;i++)
- {
- value=(((int16_t)(i2c_gyro_data[i*2+3]+(i2c_gyro_data[i*2+2]<<8)))*7)/115;// format 12|4
- ram_data[BIOLOID_IMU_GYRO_X_L+i*2]=value&0xFF;
- ram_data[BIOLOID_IMU_GYRO_X_H+i*2]=(value>>8);
- }
-}
-
diff --git a/src/imu_9dof_dma.c b/src/imu_9dof_dma.c
deleted file mode 100644
index f6f237263cd3dc5a20bf281636cac2ccaa22020d..0000000000000000000000000000000000000000
--- a/src/imu_9dof_dma.c
+++ /dev/null
@@ -1,340 +0,0 @@
-#include "imu_9dof_dma.h"
-#include "imu_int.h"
-#include "ADXL345_accel.h"
-#include "ITG3200_gyro.h"
-#include "HMC5843_compass.h"
-#include "ram.h"
-
-// I2C private variables
-volatile uint8_t i2c_error;
-volatile i2c_op_t i2c_op;
-uint8_t i2c_rd_addr;
-uint8_t i2c_wr_addr;
-uint8_t i2c_data_out[32];
-uint8_t imu_stop_flag;
-volatile uint8_t imu_stopped;
-// dma initialization structures;
-DMA_InitTypeDef IMU_DMA_TX_InitStructure;
-DMA_InitTypeDef IMU_DMA_RX_InitStructure;
-
-// 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;
-}
-
-// interrupt handlers
-void IMU_TIMER_IRQHandler(void)
-{
- static imu_state_t state=IMU_GET_COMPASS;
- uint16_t capture;
-
- if(TIM_GetITStatus(IMU_TIMER, TIM_IT_CC1)!=RESET)
- {
- TIM_ClearITPendingBit(IMU_TIMER,TIM_IT_CC1);
- capture = TIM_GetCapture1(IMU_TIMER);
- TIM_SetCompare1(IMU_TIMER, capture + 1000);
- if(imu_is_op_done())
- {
- if(imu_stop_flag==0x01)
- {
- // stop the timer to get the imu data
- TIM_Cmd(IMU_TIMER, DISABLE);
- TIM_ITConfig(IMU_TIMER, TIM_IT_CC1, DISABLE);
-
- ram_clear_bit(BIOLOID_IMU_STATUS,0);
- imu_stop_flag=0x00;
- imu_stopped=0x01;
- }
- else
- {
- switch(state)
- {
- case IMU_GET_ACCEL: imu_accel_get_data();
- imu_compass_process_data();
- state=IMU_GET_GYRO;
- break;
- case IMU_GET_GYRO: imu_gyro_get_data();
- imu_accel_process_data();
- state=IMU_GET_COMPASS;
- break;
- case IMU_GET_COMPASS: imu_compass_get_data();
- imu_gyro_process_data();
- state=IMU_GET_ACCEL;
- break;
- }
- }
- }
- }
-}
-
-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,i2c_rd_addr,I2C_Direction_Receiver);
- else
- I2C_Send7bitAddress(I2C_IMU,i2c_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);
- /* enable DMA transfer */
- if(IMU_DMA_RX_InitStructure.DMA_BufferSize==1)
- I2C_AcknowledgeConfig(I2C_IMU,DISABLE);
- else
- I2C_AcknowledgeConfig(I2C_IMU,ENABLE);
- I2C_DMALastTransferCmd(I2C_IMU,ENABLE);
- DMA_Cmd(I2C_IMU_DMA_STREAM_RX, ENABLE);
- break;
-
- case I2C_EVENT_MASTER_BYTE_TRANSMITTED:
- if(i2c_op==I2C_WR_OP)
- {
- i2c_op=I2C_OP_DONE;
- I2C_GenerateSTOP(I2C_IMU, 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;
- }
-}
-
-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;
-}
-
-// public functions
-void imu_init(void)
-{
- uint8_t i;
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
- 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);
- /* Enable the IMU timer clock */
- RCC_APB1PeriphClockCmd(IMU_TIMER_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_Enable;
- 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 = 3;
- 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 = 3;
- 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;
- imu_stop_flag=0x00;
- imu_stopped=0x00;
- for(i=0;i<32;i++)
- i2c_data_out[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 = 3;
- 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 = 3;
- 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);
-
- // configure the timer
- // initialize the timer interrupts
- NVIC_InitStructure.NVIC_IRQChannel = IMU_TIMER_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- /* motion timer configuration */
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_Prescaler = 82;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(IMU_TIMER,&TIM_TimeBaseStructure);
- TIM_SetClockDivision(IMU_TIMER,TIM_CKD_DIV1);
-
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- TIM_OCInitStructure.TIM_Pulse = 1000;// 1ms period
- TIM_OC1Init(IMU_TIMER, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(IMU_TIMER, TIM_OCPreload_Disable);
-
- /* detect the three sensors in the imu */
- if(imu_accel_detect())
- ram_set_bit(BIOLOID_IMU_STATUS,1);
- else
- ram_clear_bit(BIOLOID_IMU_STATUS,1);
- if(imu_gyro_detect())
- ram_set_bit(BIOLOID_IMU_STATUS,2);
- else
- ram_clear_bit(BIOLOID_IMU_STATUS,2);
- if(imu_compass_detect())
- ram_set_bit(BIOLOID_IMU_STATUS,3);
- else
- ram_clear_bit(BIOLOID_IMU_STATUS,3);
-}
-
-void imu_start(void)
-{
- // start the accelerometer
- imu_accel_start();
- imu_gyro_start();
- imu_compass_start();
-
- // start the timer to get the imu data
- TIM_Cmd(IMU_TIMER, ENABLE);
- TIM_ITConfig(IMU_TIMER, TIM_IT_CC1, ENABLE);
-
- ram_set_bit(BIOLOID_IMU_STATUS,0);
-}
-
-void imu_stop(void)
-{
- imu_stop_flag=0x01;
- while(!imu_stopped);
- imu_stopped=0x00;
- imu_accel_stop();
- imu_gyro_stop();
- imu_compass_stop();
-}