/*==============================================================================================*/
u8 MPU9150_Init(void)
{
u8 i = 0;
u8 ReadBuf[3] = {0};
u8 MPU6050_Init_Data[6][2] = {
{0x01, MPU6050_PWR_MGMT_1}, // Reset device
{0x03, MPU6050_CONFIG}, //
{0x18, MPU6050_GYRO_CONFIG}, // +-2000dps
{0x08, MPU6050_ACCEL_CONFIG}, // +-4G
{0x32, MPU6050_INT_PIN_CFG}, //
{0x00, MPU6050_USER_CTRL} //
};
u8 AK8975_Init_Data[3][2] = {
{0x0F, AK8975_CNTL}, // Set Fuse ROM access mode
{0x00, AK8975_CNTL}, // Set Power Down Mode
{0x01, AK8975_CNTL} // Set Single Measurement Mode
};
/* MPU6050 */
I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_WHO_AM_I, ReadBuf, 1);
if (ReadBuf[0] != MPU6050_Device_ID)
return ERROR;
Delay_1ms(10);
for (i = 0; i < 6; i++) {
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_Init_Data[i][1], MPU6050_Init_Data[i], 1);
Delay_1ms(10);
}
/* AK8975 */
I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_WIA, ReadBuf, 1);
if (ReadBuf[0] != AK8975_Device_ID)
return ERROR;
I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[0][1], AK8975_Init_Data[0], 1);
Delay_1ms(10);
I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_ASAX, ReadBuf, 3);
Delay_1ms(10);
Mag.AdjustX = ((ReadBuf[0] + 128) * 30) >> 8;
Mag.AdjustY = ((ReadBuf[1] + 128) * 30) >> 8;
Mag.AdjustZ = ((ReadBuf[2] + 128) * 30) >> 8;
I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[1][1], AK8975_Init_Data[1], 1);
Delay_1ms(10);
I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[2][1], AK8975_Init_Data[2], 1);
Delay_1ms(10);
return SUCCESS;
}
/*==============================================================================================*/
void MPU9150_Read( u8* ReadBuf )
{
u8 ReadData = 0x00;
u8 WriteData = 0x01;
I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, ReadBuf, 14);
I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_ST1, &ReadData, 1); // Wait Prapare Data
if(ReadData == 1) {
I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_HXL, ReadBuf+14, 6);
I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_CNTL, &WriteData, 1); // Set Single Measurement Mode
}
}
/* 初始化所有板载传感器 */
unsigned int sensors_init(void)
{
unsigned char temp;
//first of all we should initialize the i2c mutex
i2c1_mutex_init();
rt_thread_delay(100);
//initialization of the ms5611
i2c1_take(RT_WAITING_FOREVER);
ms5611_parameter_get();
i2c1_release();
//take the i2c bus
i2c1_take(RT_WAITING_FOREVER);
//HMC5883L initialization. ID should be 0x48
I2C_DMA_ReadReg(&HMC_ID, HMC5883_RD_ADDR, HMC_ID_REG_A, 1 );
rt_thread_delay(5);
temp = 0x78;
I2C_DMA_WriteReg(&temp, HMC5883_WR_ADDR, CFG_A , 1); //75HZ out put rate
temp = 0x20;
I2C_DMA_WriteReg(&temp, HMC5883_WR_ADDR, CFG_B , 1); //1.3GASS
temp = 0x00;
I2C_DMA_WriteReg(&temp, HMC5883_WR_ADDR, MODE , 1); //continue measurment mode
//MPU6050 initialization. ID should be 0x68
I2C_DMA_ReadReg(&MPU_ID, MPU6050_RD_ADDRESS, MPU6050_WHO_AM_I, 1 );
rt_thread_delay(5);
temp = 0x80;
I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x6B, 1); //PWR_MGMT_1 -- DEVICE_RESET 1
temp = 0x00;
I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x19, 1); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
temp = 0x00;
I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x1A, 1); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
temp = 0x03;
I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x6B, 1); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
temp = 0x00;
I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x1B, 1); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 250 deg/sec
temp = 0x00;
I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x1C, 1); //ACC_CONFIG -- FS_SEL = 3: Full scale set to 2G
//release the i2c bus
i2c1_release();
return 0;
}
/*
* Name : read_imu_fast
* Description : ---
* Author : lynx@sia.
*
* History
* ----------------------
* Rev : 0.00
* Date : 10/20/2013
*
* create.
* ----------------------
*/
static int read_imu_fast(void)
{
u8 IMU_Buf[20] = {0};
I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, IMU_Buf, 14);
// MS5611_ReadADC(&imu_sensor.Bar);
// MS5611_Calculate(&imu_sensor.Bar);
imu_sensor.Acc.X = (s16)((IMU_Buf[0] << 8) | IMU_Buf[1]);
imu_sensor.Acc.Y = (s16)((IMU_Buf[2] << 8) | IMU_Buf[3]);
imu_sensor.Acc.Z = (s16)((IMU_Buf[4] << 8) | IMU_Buf[5]);
// Tmp = (s16)((IMU_Buf[6] << 8) | IMU_Buf[7]);
imu_sensor.Gyr.X = (s16)((IMU_Buf[8] << 8) | IMU_Buf[9]);
imu_sensor.Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
imu_sensor.Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
//计算真值
imu_sensor.Acc.TrueX = imu_sensor.Acc.X/imu_sensor.Acc.RawToTrue;
imu_sensor.Acc.TrueY = imu_sensor.Acc.Y/imu_sensor.Acc.RawToTrue;
imu_sensor.Acc.TrueZ = imu_sensor.Acc.Z/imu_sensor.Acc.RawToTrue;
//计算真值
imu_sensor.Gyr.TrueX = imu_sensor.Gyr.X/imu_sensor.Gyr.RawToTrue;
imu_sensor.Gyr.TrueY = imu_sensor.Gyr.Y/imu_sensor.Gyr.RawToTrue;
imu_sensor.Gyr.TrueZ = imu_sensor.Gyr.Z/imu_sensor.Gyr.RawToTrue;
return 0;
}
/*==============================================================================================*/
void MPU6050_Read( s16* IMU_Buf )
{
u8 ReadBuf[14] = {0};
I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, ReadBuf, 14);
IMU_Buf[0] = (s16)((ReadBuf[0] << 8) | ReadBuf[1]);
IMU_Buf[1] = (s16)((ReadBuf[2] << 8) | ReadBuf[3]);
IMU_Buf[2] = (s16)((ReadBuf[4] << 8) | ReadBuf[5]);
IMU_Buf[3] = (s16)((ReadBuf[6] << 8) | ReadBuf[7]);
IMU_Buf[4] = (s16)((ReadBuf[8] << 8) | ReadBuf[9]);
IMU_Buf[5] = (s16)((ReadBuf[10] << 8) | ReadBuf[11]);
IMU_Buf[6] = (s16)((ReadBuf[12] << 8) | ReadBuf[13]);
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
u8 IMU_Buf[16] = {0};
/* 400Hz 取樣 */
I2C_DMA_ReadReg(IMU_Buf, MPU6050_I2C_ADDR, 0x3B, 14);
/* 資料合併 */
Acc.X = (s16)((IMU_Buf[0] << 8) | IMU_Buf[1]);
Acc.Y = (s16)((IMU_Buf[2] << 8) | IMU_Buf[3]);
Acc.Z = (s16)((IMU_Buf[4] << 8) | IMU_Buf[5]);
Gyr.X = (s16)((IMU_Buf[8] << 8) | IMU_Buf[9]);
Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
// Meg.X = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
// Meg.Y = (s16)((IMU_Buf[14] << 8) | IMU_Buf[15]);
// Meg.Z = (s16)((IMU_Buf[16] << 8) | IMU_Buf[17]);
}
/*==============================================================================================*/
void ADXL345_Read_Multiple( u8* ReadMultiple )
{
I2C_DMA_ReadReg(ReadMultiple, ADXL345_I2C_ADDR, ADXL345_MULTIPLE, 6);
}
