diff --git a/include/ADXL345_accel.h b/include/ADXL345_accel.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2e2ebcf40b7b8a2d06db8bafed3447f219e800a
--- /dev/null
+++ b/include/ADXL345_accel.h
@@ -0,0 +1,15 @@
+#ifndef ADXL345_ACCEL_H
+#define ADXL345_ACCEL_H
+
+#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);
+
+#endif
diff --git a/include/HMC5843_compass.h b/include/HMC5843_compass.h
new file mode 100644
index 0000000000000000000000000000000000000000..eac9ddab5bad5c68687fe636c6849ca9821b8da2
--- /dev/null
+++ b/include/HMC5843_compass.h
@@ -0,0 +1,15 @@
+#ifndef _HMC5843_compass_h
+#define _HMC5843_compass_h
+
+#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);
+
+
+#endif
diff --git a/include/ITG3200_gyro.h b/include/ITG3200_gyro.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2efc4c83c57a110346374a9d6cab61a758a483f
--- /dev/null
+++ b/include/ITG3200_gyro.h
@@ -0,0 +1,15 @@
+#ifndef _ITG3200_GYRO_H
+#define _ITG3200_GYRO_H
+
+#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);
+
+#endif
diff --git a/include/imu_9dof_dma.h b/include/imu_9dof_dma.h
index 8163d5341d33605b433b0cf5c608e8ac3fbff6fc..41db3c0d74587c695e0ae1e224e3718172f5aecc 100644
--- a/include/imu_9dof_dma.h
+++ b/include/imu_9dof_dma.h
@@ -3,49 +3,9 @@
#include "stm32f4xx.h"
-// 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
-
// general IMU public functions
void imu_init(void);
void imu_start(void);
void imu_stop(void);
-// gyroscope public functions
-uint8_t imu_gyro_detect(void);
-
-// compass public functions
-uint8_t imu_compass_detect(void);
-
#endif
diff --git a/include/imu_int.h b/include/imu_int.h
new file mode 100644
index 0000000000000000000000000000000000000000..9dc552e2a819ea55f78399fb0242d2db4f632fdd
--- /dev/null
+++ b/include/imu_int.h
@@ -0,0 +1,67 @@
+#ifndef _IMU_INT_H
+#define _IMU_INT_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
+
+#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);
+
+#endif
diff --git a/include/ram.h b/include/ram.h
index d9378534607db8f8b1d861a65a4822f0e6199b3b..5705338913943b95afa0279dddc9ca9a74fbcd5b 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -123,9 +123,10 @@ typedef enum {
BIOLOID_IMU_COMPASS_Y_H = 0x7D,
BIOLOID_IMU_COMPASS_Z_L = 0x7E,
BIOLOID_IMU_COMPASS_Z_H = 0x7F,
- BIOLOID_ACTION_PAGE = 0x80,
- BIOLOID_ACTION_CONTROL = 0x81,
- BIOLOID_ACTION_SATUTUS = 0x82
+ BIOLOID_IMU_TEMP = 0x80,
+ BIOLOID_ACTION_PAGE = 0x81,
+ BIOLOID_ACTION_CONTROL = 0x82,
+ BIOLOID_ACTION_SATUTUS = 0x83
} bioloid_registers;
#define RAM_SIZE 256
diff --git a/src/ADXL345_accel.c b/src/ADXL345_accel.c
new file mode 100644
index 0000000000000000000000000000000000000000..1251cb34ddc635a46a4ddf88976b078338de7af2
--- /dev/null
+++ b/src/ADXL345_accel.c
@@ -0,0 +1,220 @@
+#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
new file mode 100644
index 0000000000000000000000000000000000000000..da5b65fec74851b58e62bd8fa847df0ee61b324b
--- /dev/null
+++ b/src/HMC5843_compass.c
@@ -0,0 +1,181 @@
+#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]+(i2c_compass_data[i*2+1]<<8)))<<4)/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
new file mode 100644
index 0000000000000000000000000000000000000000..241c3ae6f291d6a46fb92e140fd565b485fb45dd
--- /dev/null
+++ b/src/ITG3200_gyro.c
@@ -0,0 +1,219 @@
+#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/bioloid_stm32.c b/src/bioloid_stm32.c
index 6035dfd64006ca2a50b25fec47e4221522593ff0..d7f18f8c03243b2ad02bb87b1e2cf2ca5220c3e3 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -147,7 +147,7 @@ int32_t main(void)
EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
dyn_slave_set_return_level((uint8_t)eeprom_data);
/* initialize the IMU */
-// imu_init();
+ imu_init();
// initialize the Analog to digital converter
// adc_init();
// initialize motion manager
diff --git a/src/imu_9dof_dma.c b/src/imu_9dof_dma.c
index 5099fb129579d5e3f38ca383feb9e8b7872df831..6f166fa72645bbb4bc1af2657b6690dcd7b6f935 100644
--- a/src/imu_9dof_dma.c
+++ b/src/imu_9dof_dma.c
@@ -1,128 +1,16 @@
#include "imu_9dof_dma.h"
+#include "imu_int.h"
+#include "ADXL345_accel.h"
+#include "ITG3200_gyro.h"
+#include "HMC5843_compass.h"
#include "ram.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
-
-#define IMU_TIMER TIM4
-#define IMU_TIMER_IRQn TIM4_IRQn
-#define IMU_TIMER_IRQHandler TIM4_IRQHandler
-#define IMU_TIMER_CLK RCC_APB1Periph_TIM4
-
-// 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
-
-
-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;
-
-// private variables
// 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];
-// 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;
-// 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_data_reg=0x1B;
-const uint8_t gyro_data_len=0x08;
-const uint8_t gyro_rd_addr=0xD0;
-const uint8_t gyro_wr_addr=0xD1;
-uint8_t i2c_gyro_conf[4];
-uint8_t i2c_gyro_data[8];
-// 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_rd_addr=0x3D;
-const uint8_t compass_wr_addr=0x3C;
-uint8_t i2c_compass_conf[4];
-uint8_t i2c_compass_data[6];
// dma initialization structures;
DMA_InitTypeDef IMU_DMA_TX_InitStructure;
DMA_InitTypeDef IMU_DMA_RX_InitStructure;
@@ -141,145 +29,6 @@ uint8_t imu_is_op_done(void)
return 0x00;
}
-// private 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;
- 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_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);
- }
-}
-
// interrupt handlers
void IMU_TIMER_IRQHandler(void)
{
@@ -296,12 +45,16 @@ void IMU_TIMER_IRQHandler(void)
switch(state)
{
case IMU_GET_ACCEL: imu_accel_get_data();
+ imu_compass_process_data();
state=IMU_GET_GYRO;
break;
- case IMU_GET_GYRO: imu_accel_process_data();
+ case IMU_GET_GYRO: imu_gyro_get_data();
+ imu_accel_process_data();
state=IMU_GET_COMPASS;
break;
- case IMU_GET_COMPASS: state=IMU_GET_ACCEL;
+ case IMU_GET_COMPASS: imu_compass_get_data();
+ imu_gyro_process_data();
+ state=IMU_GET_ACCEL;
break;
}
}
@@ -390,52 +143,6 @@ void I2C_IMU_RX_DMA_IRQHandler(void)
i2c_op=I2C_OP_DONE;
}
-// gyroscope public 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)
- return 0x01;
- else
- return 0x00;
-}
-
-// 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])
- return 0x01;
- else
- return 0x00;
-}
-
// public functions
void imu_init(void)
{
@@ -595,6 +302,8 @@ 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);
@@ -611,6 +320,8 @@ void imu_stop(void)
// stop the accelerometer
imu_accel_stop();
+ imu_gyro_stop();
+ imu_compass_stop();
ram_clear_bit(BIOLOID_IMU_STATUS,0);
}