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; }