diff --git a/include/imu_9dof_dma.h b/include/imu_9dof_dma.h
index 993d8ac3e0321bb031b66fa15afe5031dc5cd978..3ece2d8ceb7f310dccbf9edecc6010a18fe5c852 100644
--- a/include/imu_9dof_dma.h
+++ b/include/imu_9dof_dma.h
@@ -46,4 +46,10 @@ void imu_accel_justify_left(void);
void imu_accel_justify_right(void);
void imu_accel_config(void);
+// gyroscope public functions
+uint8_t imu_gyro_detect(void);
+
+// compass public functions
+uint8_t imu_compass_detect(void);
+
#endif
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index c9917ba99788c9e79e18a3e9c6ecc1a34015465d..702a1d02d548e8272fd2b5114d4e5b544778749d 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -74,10 +74,15 @@ int32_t main(void)
GPIO_SetBits(GPIOD,GPIO_Pin_12);
else
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
-
- gpio_set_pushbutton_callback(NORTH_PB,test_led);
- gpio_blink_led(EAST_LED,2000);
- gpio_blink_led(WEST_LED,1000);
+ if(imu_gyro_detect())
+ GPIO_SetBits(GPIOD,GPIO_Pin_13);
+ else
+ GPIO_ResetBits(GPIOD,GPIO_Pin_13);
+ if(imu_compass_detect())
+ GPIO_SetBits(GPIOD,GPIO_Pin_14);
+ else
+ GPIO_ResetBits(GPIOD,GPIO_Pin_14);
+
while(1) /* main function does not return */
{
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
diff --git a/src/imu_9dof_dma.c b/src/imu_9dof_dma.c
index 8144b635bd436dae0cbcbc63e03cda778bc5a770..2d595a2941485d813543706e241e6646274a487f 100644
--- a/src/imu_9dof_dma.c
+++ b/src/imu_9dof_dma.c
@@ -49,6 +49,8 @@ volatile uint8_t i2c_error;
volatile i2c_op_t i2c_op;
uint8_t i2c_conf[32];
uint8_t i2c_data[32];
+uint8_t i2c_rd_addr;
+uint8_t i2c_wr_addr;
// accelerometer data and configuration
const uint8_t accel_id_reg=0x00;
const uint8_t accel_id=0xE5;
@@ -59,6 +61,26 @@ const uint8_t accel_data_reg=0x32;
const uint8_t accel_data_len=0x06;
const uint8_t accel_rd_addr=0xA6;
const uint8_t accel_wr_addr=0xA7;
+// 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;
+// 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;
// dma initialization structures;
DMA_InitTypeDef IMU_DMA_TX_InitStructure;
DMA_InitTypeDef IMU_DMA_RX_InitStructure;
@@ -71,9 +93,9 @@ void I2C_IMU_EV_IRQHandler(void)
/* Test on I2C1 EV5 and clear it */
case I2C_EVENT_MASTER_MODE_SELECT:
if(i2c_op==I2C_RD_OP_DATA)
- I2C_Send7bitAddress(I2C_IMU,accel_rd_addr,I2C_Direction_Receiver);
+ I2C_Send7bitAddress(I2C_IMU,i2c_rd_addr,I2C_Direction_Receiver);
else
- I2C_Send7bitAddress(I2C_IMU,accel_wr_addr,I2C_Direction_Transmitter);
+ I2C_Send7bitAddress(I2C_IMU,i2c_wr_addr,I2C_Direction_Transmitter);
break;
/* Test on I2C1 EV6 and first EV8 and clear them */
@@ -86,9 +108,12 @@ void I2C_IMU_EV_IRQHandler(void)
case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED:
I2C_ITConfig(I2C_IMU, I2C_IT_EVT, DISABLE);
- I2C_AcknowledgeConfig(I2C_IMU,DISABLE);
/* enable DMA transfer */
- I2C_DMACmd(I2C_IMU, ENABLE);
+ 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;
@@ -154,6 +179,7 @@ uint8_t imu_is_op_done(void)
return 0x00;
}
+// accelerometer public functions
uint8_t imu_accel_detect(void)
{
i2c_op=I2C_RD_OP_CMD;
@@ -165,6 +191,8 @@ uint8_t imu_accel_detect(void)
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
i2c_conf[0]=accel_id_reg;
i2c_data[0]=0x00;
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
@@ -223,7 +251,57 @@ void imu_accel_config(void)
i2c_conf[0]=accel_conf_reg;
i2c_conf[15]=0x0A;
i2c_conf[20]=0x02;
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+}
+
+// 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_data;
+ DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_conf[0]=gyro_id_reg;
+ i2c_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_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_data;
+ DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_conf[0]=compass_id_reg;
+ i2c_data[0]=0x00;
+ i2c_data[1]=0x00;
+ i2c_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_data[0]==compass_id[0] && i2c_data[1]==compass_id[1] && i2c_data[2]==compass_id[2])
+ return 0x01;
+ else
+ return 0x00;
}
// public functions
@@ -268,7 +346,7 @@ void imu_init(void)
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0x00;
- I2C_InitStructure.I2C_Ack = I2C_Ack_Disable;
+ I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = 400000;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_Init(I2C_IMU, &I2C_InitStructure);