uint8_t MPU9250_Init( MPU_InitTypeDef *MPUx )
{
uint8_t status = ERROR;
uint8_t MPU6500_InitData[MPU6500_InitRegNum][2] = {
{0x80, MPU6500_PWR_MGMT_1}, // Reset Device
{0x01, MPU6500_PWR_MGMT_1}, // Clock Source
{0x30, MPU6500_USER_CTRL}, // Set I2C_MST_EN, I2C_IF_DIS
{0x00, MPU6500_PWR_MGMT_2}, // Enable Acc & Gyro
{0x07, MPU6500_CONFIG}, // DLPF_CFG[2:0] = 111;
{0x18, MPU6500_GYRO_CONFIG}, // +-2000dps
{0x08, MPU6500_ACCEL_CONFIG}, // +-4G
{0x48, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
{0x10, MPU6500_INT_PIN_CFG}, // Set INT_ANYRD_2CLEAR
{0x10, MPU6500_INT_ENABLE}, //
{0x4D, MPU6500_I2C_MST_CTRL}, // I2C Speed 400 kHz
};
uint8_t AK8963_InitData[AK8963_InitRegNum][2] = {
{0x01, AK8963_CNTL2}, // Reset Device
{0x06, AK8963_CNTL1}, // Continuous measurement mode 2
};
MPU6500_InitData[5][0] = MPUx->MPU_Gyr_FullScale; // MPU6500_GYRO_CONFIG
MPU6500_InitData[4][0] = MPUx->MPU_Gyr_LowPassFilter; // MPU6500_CONFIG
MPU6500_InitData[6][0] = MPUx->MPU_Acc_FullScale; // MPU6500_ACCEL_CONFIG
MPU6500_InitData[7][0] = MPUx->MPU_Acc_LowPassFilter; // MPU6500_ACCEL_CONFIG_2
AK8963_InitData[1][0] |= MPUx->MPU_Mag_FullScale;
for(uint8_t i = 0; i < MPU6500_InitRegNum; i++) {
MPU9250_WriteReg(MPU6500_InitData[i][1], MPU6500_InitData[i][0]);
Delay_1ms(1);
}
status = MPU9250_Check();
if(status != SUCCESS)
return ERROR;
Delay_10ms(1);
#ifdef _USE_MAG_AK8963
for(uint8_t i = 0; i < AK8963_InitRegNum; i++) {
MPU9250_Mag_WriteReg(AK8963_InitData[i][1], AK8963_InitData[i][0]);
Delay_10ms(1);
}
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_ST1);
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x80 | 8);
#endif
MPU9250_SetSpeed(SPIx_SPEED_HIGH);
Delay_10ms(1);
return SUCCESS;
}
void MPU9250_Init( MPU_InitTypeDef *MPUx )
{
u8 i = 0;
u8 MPU6500_Init_Data[MPU9250_InitRegNum][2] = {
{0x10, MPU6500_USER_CTRL}, //
{0x00, MPU6500_PWR_MGMT_1}, //
{0x00, MPU6500_PWR_MGMT_2}, // Enable Acc & Gyro
{0x07, MPU6500_CONFIG}, // DLPF_CFG[2:0] = 111;
{0x18, MPU6500_GYRO_CONFIG}, // +-2000dps
{0x08, MPU6500_ACCEL_CONFIG}, // +-4G
{0x48, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
{0x30, MPU6500_INT_PIN_CFG}, //
{0x4D, MPU6500_I2C_MST_CTRL}, // I2C Speed 400 kHz
// // Set Slave to Read AK8963
// {0x8C, MPU6500_I2C_SLV0_ADDR}, // AK8963_I2C_ADDR ( 7'b000_1100 )
// {0x00, MPU6500_I2C_SLV0_REG}, // AK8963_WIA ( 0x00 )
// {0x81, MPU6500_I2C_SLV0_CTRL}, // Enable
// {0x01, MPU6500_I2C_MST_DELAY_CTRL}
};
MPU6500_Init_Data[4][0] = MPUx->MPU_Gyr_FullScale; // MPU6500_GYRO_CONFIG
MPU6500_Init_Data[3][0] = MPUx->MPU_Gyr_LowPassFilter; // MPU6500_CONFIG
MPU6500_Init_Data[5][0] = MPUx->MPU_Acc_FullScale; // MPU6500_ACCEL_CONFIG
MPU6500_Init_Data[6][0] = MPUx->MPU_Acc_LowPassFilter; // MPU6500_ACCEL_CONFIG_2
LED_B = 0;
for(i=0; i<MPU9250_InitRegNum; i++) {
MPU9250_WriteReg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
LED_G = 0;
Delay_1ms(5);
}
while(MPU9250_Check() != SUCCESS);
MPU9250_SetSpeedSPI(SPI_SPEED_HIGH);
}
uint8_t MPU9250_Init( MPU_InitTypeDef *MPUx )
{
uint8_t status = ERROR;
#ifdef _USE_MAG_AK8963
uint8_t tmpRead[3] = {0};
#endif
uint8_t MPU6500_InitData[MPU6500_InitRegNum][2] = {
{0x80, MPU6500_PWR_MGMT_1}, // [0] Reset Device
{0x04, MPU6500_PWR_MGMT_1}, // [1] Clock Source
{0x10, MPU6500_INT_PIN_CFG}, // [2] Set INT_ANYRD_2CLEAR
{0x01, MPU6500_INT_ENABLE}, // [3] Set RAW_RDY_EN
{0x00, MPU6500_PWR_MGMT_2}, // [4] Enable Acc & Gyro
{0x00, MPU6500_SMPLRT_DIV}, // [5] Sample Rate Divider
{0x18, MPU6500_GYRO_CONFIG}, // [6] default : +-2000dps
{0x08, MPU6500_ACCEL_CONFIG}, // [7] default : +-4G
{0x07, MPU6500_CONFIG}, // [8] default : LPS_41Hz
{0x03, MPU6500_ACCEL_CONFIG_2}, // [9] default : LPS_41Hz
{0x30, MPU6500_USER_CTRL}, // [10] Set I2C_MST_EN, I2C_IF_DIS
};
MPU6500_InitData[6][0] = MPUx->MPU_Gyr_FullScale; // MPU6500_GYRO_CONFIG
MPU6500_InitData[8][0] = MPUx->MPU_Gyr_LowPassFilter; // MPU6500_CONFIG
MPU6500_InitData[7][0] = MPUx->MPU_Acc_FullScale; // MPU6500_ACCEL_CONFIG
MPU6500_InitData[9][0] = MPUx->MPU_Acc_LowPassFilter; // MPU6500_ACCEL_CONFIG_2
for(uint8_t i = 0; i < MPU6500_InitRegNum; i++) {
Delay_1ms(2);
MPU9250_WriteReg(MPU6500_InitData[i][1], MPU6500_InitData[i][0]);
}
status = MPU9250_Check();
if(status != SUCCESS)
return ERROR;
Delay_10ms(1);
#ifdef _USE_MAG_AK8963
MPU9250_Mag_WriteReg(AK8963_CNTL2, 0x01); // Reset Device
Delay_1ms(1);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x10); // Power-down mode
Delay_1ms(1);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x1F); // Fuse ROM access mode
Delay_1ms(1);
MPU9250_Mag_ReadRegs(AK8963_ASAX, tmpRead, 3); // Read sensitivity adjustment values
Delay_1ms(1);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x10); // Power-down mode
Delay_1ms(1);
AK8963_ASA[0] = (int16_t)(tmpRead[0]) + 128;
AK8963_ASA[1] = (int16_t)(tmpRead[1]) + 128;
AK8963_ASA[2] = (int16_t)(tmpRead[2]) + 128;
MPU9250_WriteReg(MPU6500_I2C_MST_CTRL, 0x5D);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_ST1);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, MPU6500_I2C_SLVx_EN | 8);
Delay_1ms(1);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x16); // Continuous measurement mode 2
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, 0x09);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_MST_DELAY_CTRL, 0x81);
Delay_100ms(1);
#endif
MPU9250_SetSpeed(SPIx_SPEED_HIGH);
Delay_10ms(1);
return SUCCESS;
}
