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