void MPU9250_WriteAuxReg(uint32_t address, uint32_t data)
{
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, 0x0C); // Set AK8963_I2C_ADDR = 7'b000_1100
MPU9250_Delay_nop(10000);
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, address); // Set Write Reg
MPU9250_WriteReg(MPU6500_I2C_SLV0_DO, data); // Read Data
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x81); // Start Read
MPU9250_Delay_nop(10000);
}
uint32_t MPU9250_ReadAuxReg(uint32_t address)
{
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, 0x8C); // Set AK8963_I2C_ADDR = 7'b000_1100
//MPU9250_Delay_nop(100000);
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, address); // Set Write Reg
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x81); // Start Read
MPU9250_Delay_nop(10000);
return MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_00); // Read Data
}
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;
}
/*=====================================================================================================*/
static void MPU9250_Mag_WriteReg( uint8_t WriteAddr, uint8_t WriteData )
{
uint8_t timeout = 0;
uint8_t status = 0;
MPU9250_WriteReg(MPU6500_I2C_SLV4_REG, WriteAddr);
MPU9250_WriteReg(MPU6500_I2C_SLV4_ADDR, AK8963_I2C_ADDR);
MPU9250_WriteReg(MPU6500_I2C_SLV4_DO, WriteData);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, 0x80);
do {
status = MPU9250_ReadReg(MPU6500_I2C_MST_STATUS);
} while(((status & 0x40) == 0) && (timeout++ < 50));
}
bool MPU9250_GetMag()
{
bool available;
available = false;
#ifdef USE_MAG
int16_t temp;
int16_t temp2;
int16_t temp3;
temp = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_00); // read AK8963_ST1
if (temp & 0x01)
{
available = true;
temp = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_01);
temp2 = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_02)<<8;
temp3 = temp + temp2;
Mag.X = (int32_t) (temp3);
temp = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_03);
temp2 = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_04)<<8;
temp3 = temp + temp2;
Mag.Y = (int32_t) (temp3);
temp = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_05);
temp2 = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_06)<<8;
temp3 = temp + temp2;
Mag.Z = (int32_t) (temp3);
temp = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_07);
Mag.X = Xs * Mag.X + Xb;
Mag.Y = Ys * Mag.Y + Yb;
// Mag.Z =
if (temp & 0x08) // magnetic sensor overflow occurred
{
Mag.X = 0;
Mag.Y = 0;
Mag.Z = 0;
available = false;
}
temp = MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_08);
}
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, 0x8C); // Set AK8963_I2C_ADDR = 7'b000_1100
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_ST1); // Set Write Reg
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x89); // Start Read
#endif
return available;
}
void MPU9250_SetParameter( void )
{
uint8_t i = 0;
uint32_t MPU6500_Init_Data[MPU9250_InitRegNum][2] = {
{0x80, MPU6500_PWR_MGMT_1}, // REG107 Reset Device
{0x01, MPU6500_PWR_MGMT_1}, // REG107 Clock Source
{0x00, MPU6500_PWR_MGMT_2}, // REG108 Enable Acc & Gyro
{0x04, MPU6500_CONFIG}, // REG26 FCHOICE=11 DLPF=2 Gyro Bandwidth 92Hz Rates 1K
{0x18, MPU6500_GYRO_CONFIG}, // REG27 +-2000dps
{0x00, MPU6500_ACCEL_CONFIG}, // REG28 +-2G
{0x03, MPU6500_ACCEL_CONFIG_2}, // REG29 Set Acc Data Rates ACCEL Bandwidth 92Hz Rates 1K
{0x30, MPU6500_INT_PIN_CFG}, // REG55 BYPASS_EN
{0x40, MPU6500_I2C_MST_CTRL}, // REG36 I2C Speed 348 kHz
{0x20, MPU6500_USER_CTRL}, // REG106 Enable AUX
// Set Slave to Read AK8963
{0x8C, MPU6500_I2C_SLV0_ADDR}, // REG37 AK8963_I2C_ADDR ( 7'b000_1100 )
{0x00, MPU6500_I2C_SLV0_REG}, // REG38 AK8963_WIA ( 0x00 )
{0x81, MPU6500_I2C_SLV0_CTRL}, // REG39 Enable
{0x01, MPU6500_I2C_MST_DELAY_CTRL} //REG103
};
for(i=0; i<MPU9250_InitRegNum; i++) {
MPU9250_WriteReg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
MPU9250_Delay_nop(10000);
}
}
/**
* @brief MPU9250_Init
* @note MPU9250 module init
* @param None
* @retval None
*/
void MPU9250_Init( void )
{
uint8_t i = 0;
uint8_t MPU6500_Init_Data[MPU9250_InitRegNum][2] = {
{0x80, MPU6500_PWR_MGMT_1}, // Reset Device
{0x01, MPU6500_PWR_MGMT_1}, // Clock Source
{0x00, MPU6500_PWR_MGMT_2}, // Enable Acc & Gyro
{0x07, MPU6500_CONFIG}, //
{0x18, MPU6500_GYRO_CONFIG}, // +-2000dps
{0x08, MPU6500_ACCEL_CONFIG}, // +-4G
{0x00, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
{0x30, MPU6500_INT_PIN_CFG}, //
{0x40, MPU6500_I2C_MST_CTRL}, // I2C Speed 348 kHz
{0x20, MPU6500_USER_CTRL}, // Enable AUX
// Set Slave to Read AK8963
{0x18, 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}
};
for(i=0; i<MPU9250_InitRegNum; i++) {
MPU9250_WriteReg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
rt_thread_delay(1);//Delay 10ms
}
}
/**
* @brief MPU9250_Check_AK8963
* @note AK8963 in MPU9250 ID check
* @param None
* @retval None
*/
uint8_t MPU9250_Check_AK8963( void )
{
uint8_t DeviceID2 = 0x00;
DeviceID2 = 0x00;
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, 0x8C); // Set AK8963_I2C_ADDR = 7'b000_1100
rt_thread_delay(1);//Delay 10ms
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_WIA); // Set Write Reg
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x81); // Start Read
rt_thread_delay(1);//Delay 10ms
MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_00, &DeviceID2); // Read Data
rt_kprintf("AK8963_ID=%d\r\n",DeviceID2);
if(DeviceID2 != AK8963_Device_ID)
return ERROR;
return SUCCESS;
}
/*=====================================================================================================*/
static uint8_t MPU9250_Mag_ReadReg( uint8_t ReadAddr )
{
uint8_t timeout = 0;
uint8_t status = 0;
uint8_t ReadData = 0;
MPU9250_WriteReg(MPU6500_I2C_SLV4_REG, ReadAddr);
MPU9250_WriteReg(MPU6500_I2C_SLV4_ADDR, AK8963_I2C_ADDR | 0x80);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, 0x80);
do {
status = MPU9250_ReadReg(MPU6500_I2C_MST_STATUS);
} while(((status & 0x40) == 0) && (timeout++ < 50));
ReadData = MPU9250_ReadReg(MPU6500_I2C_SLV4_DI);
return ReadData;
}
void MPU9250_Mag_WriteReg( uint8_t writeAddr, uint8_t writeData )
{
uint8_t status = 0;
uint32_t timeout = MAG_READ_DELAY;
MPU9250_WriteReg(MPU6500_I2C_SLV4_ADDR, AK8963_I2C_ADDR);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_REG, writeAddr);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_DO, writeData);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, MPU6500_I2C_SLVx_EN);
Delay_1ms(1);
do {
status = MPU9250_ReadReg(MPU6500_I2C_MST_STATUS);
Delay_1ms(1);
} while(((status & MPU6500_I2C_SLV4_DONE) == 0) && (timeout--));
}
void MPU9250_Mag_ReadRegs( uint8_t readAddr, uint8_t *readData, uint8_t lens )
{
uint8_t status = 0;
uint32_t timeout = MAG_READ_DELAY;
MPU9250_WriteReg(MPU6500_I2C_SLV4_ADDR, AK8963_I2C_ADDR | 0x80);
Delay_1ms(1);
for(uint8_t i = 0; i< lens; i++) {
MPU9250_WriteReg(MPU6500_I2C_SLV4_REG, readAddr + i);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, MPU6500_I2C_SLVx_EN);
Delay_1ms(1);
do {
status = MPU9250_ReadReg(MPU6500_I2C_MST_STATUS);
} while(((status & MPU6500_I2C_SLV4_DONE) == 0) && (timeout--));
readData[i] = MPU9250_ReadReg(MPU6500_I2C_SLV4_DI);
Delay_1ms(1);
}
}
uint8_t MPU9250_Mag_ReadReg( uint8_t readAddr )
{
uint8_t status = 0;
uint8_t readData = 0;
uint32_t timeout = MAG_READ_DELAY;
MPU9250_WriteReg(MPU6500_I2C_SLV4_ADDR, AK8963_I2C_ADDR | 0x80);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_REG, readAddr);
Delay_1ms(1);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, MPU6500_I2C_SLVx_EN);
Delay_1ms(1);
do {
status = MPU9250_ReadReg(MPU6500_I2C_MST_STATUS);
Delay_1ms(1);
} while(((status & MPU6500_I2C_SLV4_DONE) == 0) && (timeout--));
readData = MPU9250_ReadReg(MPU6500_I2C_SLV4_DI);
return readData;
}
/*******************************************************************************
* Function Name : MPU9250_MagnSetI2CBypass
* Description : Sets the MPU9250 I2C Bypass Enable
* Input : MPU9250_BYPASS_DISABLED/MPU9250_BYPASS_ENABLED
* Output : None
* Return : Status [ZOS_ERROR, ZOS_SUCCESS]
*******************************************************************************/
zos_result_t MPU9250_MagnSetI2CBypass(MPU9250_I2C_Bypass_t bypass)
{
uint8_t value;
if( MPU9250_ReadReg(MPU9250_I2C_MASTER_BYPASS, &value) != ZOS_SUCCESS )
return ZOS_ERROR;
value &= 0xFD;
value |= (bypass << MPU9250_BYPASS_EN);
if( MPU9250_WriteReg(MPU9250_I2C_MASTER_BYPASS, value) != ZOS_SUCCESS )
return ZOS_ERROR;
return ZOS_SUCCESS;
}
void MPU9250_Init( void )
{
u8 i = 0;
u8 MPU6500_Init_Data[MPU9250_InitRegNum][2] = {
{0x80, MPU6500_PWR_MGMT_1}, // Reset Device
{0x01, MPU6500_PWR_MGMT_1}, // Clock Source
{0x00, MPU6500_PWR_MGMT_2}, // Enable Acc & Gyro
{0x07, MPU6500_CONFIG}, //
{0x18, MPU6500_GYRO_CONFIG}, // +-2000dps
{0x08, MPU6500_ACCEL_CONFIG}, // +-4G
{0x00, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
{0x30, MPU6500_INT_PIN_CFG}, //
{0x40, MPU6500_I2C_MST_CTRL}, // I2C Speed 348 kHz
{0x20, MPU6500_USER_CTRL}, // Enable AUX
};
for(i=0; i<MPU9250_InitRegNum; i++) {
MPU9250_WriteReg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
Delay_1ms(1);
}
}
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);
}
//#define LPS25H_InitRegNum 2
uint8_t IMU_Init( IMU_InitTypeDef *IMUx )
{
uint8_t i = 0;
uint8_t status = ERROR;
uint8_t tmpRead[3] = {0};
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
};
IMU_ClaerData(IMUx->pIMU);
IMU_SetSensitivity(IMUx);
MPU6500_InitData[6][0] = IMUx->InitMPU.MPU_Gyr_FullScale; // MPU6500_GYRO_CONFIG
MPU6500_InitData[7][0] = IMUx->InitMPU.MPU_Acc_FullScale; // MPU6500_ACCEL_CONFIG
MPU6500_InitData[8][0] = IMUx->InitMPU.MPU_Gyr_LowPassFilter; // MPU6500_CONFIG
MPU6500_InitData[9][0] = IMUx->InitMPU.MPU_Acc_LowPassFilter; // MPU6500_ACCEL_CONFIG_2
for(i = 0; i < MPU6500_InitRegNum; i++) {
delay_ms(2);
MPU9250_WriteReg(MPU6500_InitData[i][1], MPU6500_InitData[i][0]);
}
status = IMU_deviceCheck(IMUx->pIMU);
if(status != SUCCESS)
return ERROR;
delay_ms(10);
if(IMUx->pIMU->MPU_Mag_Enable == MPU_Mag_ENABLE) {
MPU9250_Mag_WriteReg(AK8963_CNTL2, 0x01); // Reset Device
delay_ms(2);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x10); // Power-down mode
delay_ms(2);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x1F); // Fuse ROM access mode
delay_ms(2);
MPU9250_Mag_ReadRegs(AK8963_ASAX, tmpRead, 3); // Read sensitivity adjustment values
delay_ms(2);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x10); // Power-down mode
delay_ms(2);
IMUx->pIMU->MagASA[0] = (int16_t)(tmpRead[0]) + 128;
IMUx->pIMU->MagASA[1] = (int16_t)(tmpRead[1]) + 128;
IMUx->pIMU->MagASA[2] = (int16_t)(tmpRead[2]) + 128;
MPU9250_WriteReg(MPU6500_I2C_MST_CTRL, 0x5D);
delay_ms(2);
MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
delay_ms(2);
MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_ST1);
delay_ms(2);
MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, MPU6500_I2C_SLVx_EN | 8);
delay_ms(2);
MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x16); // Continuous measurement mode 2
delay_ms(2);
MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, 0x09);
delay_ms(2);
MPU9250_WriteReg(MPU6500_I2C_MST_DELAY_CTRL, 0x81);
delay_ms(100);
}
if(IMUx->pIMU->LPS_PresTemp_Enable == LPS_PresTemp_ENABLE) {
// for(uint8_t i = 0; i < LPS25H_InitRegNum; i++) {
// Delay_1ms(2);
// LPS25H_WriteReg(LPS25H_InitData[i][1], LPS25H_InitData[i][0]);
// }
}
IMU_SetSpeed(IMU_SPIx_SPEED_HIGH);
delay_ms(10);
return SUCCESS;
}
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;
}
