/*==============================================================================================*/
void HMC5883_Init( void )
{
u8 HMC5883_Init_Data[3] = {
0x70, /* HMC5883_REG_CONF_A */
0xA0, /* HMC5883_REG_CONF_B */
0x00 /* HMC5883_REG_MODE */
};
I2C_DMA_WriteReg(HMC5883_I2C_ADDR, HMC5883_REG_CONF_A, HMC5883_Init_Data, 1); Delay_1ms(10);
I2C_DMA_WriteReg(HMC5883_I2C_ADDR, HMC5883_REG_CONF_B, HMC5883_Init_Data+1, 1); Delay_1ms(10);
I2C_DMA_WriteReg(HMC5883_I2C_ADDR, HMC5883_REG_MODE, HMC5883_Init_Data+2, 1); Delay_1ms(10);
}
/*==============================================================================================*/
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 ADXL345_Init( void )
{
u8 ADXL345_Init_Data[7] = {
0x0B, /* ADXL345_DATA_FORMAT 测量范围,正负16g，13位模式 */
0x0C, /* ADXL345_BW_RATE 输出数据速率: 400Hz */
0x08, /* ADXL345_POWER_CTL 选择电源模式 */
0x80, /* ADXL345_INT_ENABLE 使能 DATA_READY 中断 */
0x0, /* ADXL345_OFSX X 偏移量 : -4 3.9*17/15.6 = 4.25 */
0x0, /* ADXL345_OFSX Y 偏移量 : -1 3.9* 4/15.6 = 1 */
0x0 /* ADXL345_OFSZ Z 偏移量 : +9 (1000-3.9*219)/15.6 = 9.35 */
};
I2C_DMA_WriteReg(ADXL345_Init_Data, ADXL345_I2C_ADDR, ADXL345_DATA_FORMAT, 1);
DelayMs(1);
I2C_DMA_WriteReg(ADXL345_Init_Data+1, ADXL345_I2C_ADDR, ADXL345_BW_RATE, 1);
DelayMs(1);
I2C_DMA_WriteReg(ADXL345_Init_Data+2, ADXL345_I2C_ADDR, ADXL345_POWER_CTL, 1);
DelayMs(1);
I2C_DMA_WriteReg(ADXL345_Init_Data+3, ADXL345_I2C_ADDR, ADXL345_INT_ENABLE, 1);
DelayMs(1);
I2C_DMA_WriteReg(ADXL345_Init_Data+4, ADXL345_I2C_ADDR, ADXL345_OFSX, 1);
DelayMs(1);
I2C_DMA_WriteReg(ADXL345_Init_Data+5, ADXL345_I2C_ADDR, ADXL345_OFSY, 1);
DelayMs(1);
I2C_DMA_WriteReg(ADXL345_Init_Data+6, ADXL345_I2C_ADDR, ADXL345_OFSZ, 1);
DelayMs(1);
}
/* 初始化所有板载传感器 */
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;
}
/*==============================================================================================*/
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
}
}
/*==============================================================================================*/
void AK8975_Init( void )
{
u8 i = 0;
u8 AK8975_Init_Data[3][2] = {
{0x01, MPU6050_PWR_MGMT_1}, //
{0x03, MPU6050_CONFIG}, //
{0x18, MPU6050_GYRO_CONFIG} //
};
for(i=0; i<3; i++) {
I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[i][1], AK8975_Init_Data[i], 1);
Delay_1ms(10);
}
}
/*==============================================================================================*/
u8 MPU6050_Init( void )
{
u8 i = 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} //
};
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);
}
return SUCCESS;
}
int mpu_change_range(unsigned int type, unsigned int range)
{
unsigned char reg;
unsigned char data;
//加速度计
if(type == MPU_ACC)
{
reg = 0x1c;
switch(range)
{
case 2: data = 0x00; break;
case 4: data = 0x08; break;
case 8: data = 0x10; break;
case 16:data = 0x18; break;
default:return 1;
}
}
//陀螺仪
else if(type == MPU_GYRO)
{
reg = 0x1b;
switch(range)
{
case 250: data = 0x00; break;
case 500: data = 0x08; break;
case 1000: data = 0x10; break;
case 2000: data = 0x18; break;
default:return 1;
}
}
//写入寄存器
//take the i2c bus
i2c1_take(RT_WAITING_FOREVER);
I2C_DMA_WriteReg(&data, MPU6050_WR_ADDRESS, reg, 1);
i2c1_release();
return 0;
}
/*==============================================================================================*/
void MPU6050_Init( void )
{
u8 MPU6050_Init_Data[6] = {
0x01, /* MPU6050_PWR_MGMT_1 */
0x03, /* MPU6050_CONFIG */
0x18, /* MPU6050_GYRO_CONFIG +-2000dps */
0x08, /* MPU6050_ACCEL_CONFIG +-4G */
0x32, /* MPU6050_INT_PIN_CFG */
0x00 /* MPU6050_USER_CTRL */
};
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_PWR_MGMT_1, MPU6050_Init_Data, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_CONFIG, MPU6050_Init_Data+1, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_GYRO_CONFIG, MPU6050_Init_Data+2, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_CONFIG, MPU6050_Init_Data+3, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_INT_PIN_CFG, MPU6050_Init_Data+4, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_USER_CTRL, MPU6050_Init_Data+5, 1); rt_thread_delay(rt_tick_from_millisecond(10));
}
/*==============================================================================================*/
void MPU6050_Init( void )
{
u8 MPU6050_Init_Data[6] = {
0x01, /* MPU6050_PWR_MGMT_1 */
0x03, /* MPU6050_CONFIG */
0x18, /* MPU6050_GYRO_CONFIG +-2000dps */
0x08, /* MPU6050_ACCEL_CONFIG +-4G */
0x32, /* MPU6050_INT_PIN_CFG */
0x00 /* MPU6050_USER_CTRL */
};
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_PWR_MGMT_1, MPU6050_Init_Data, 1); Delay_1ms(10);
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_CONFIG, MPU6050_Init_Data+1, 1); Delay_1ms(10);
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_GYRO_CONFIG, MPU6050_Init_Data+2, 1); Delay_1ms(10);
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_CONFIG, MPU6050_Init_Data+3, 1); Delay_1ms(10);
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_INT_PIN_CFG, MPU6050_Init_Data+4, 1); Delay_1ms(10);
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_USER_CTRL, MPU6050_Init_Data+5, 1); Delay_1ms(10);
}
