/*=====================================================================================================*/
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;
}
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
}
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;
}
/*====================================================================================================*/
u8 MPU9250_Check( void )
{
u8 DeviceID = 0x00;
/* MPU6500 */
DeviceID = 0x00;
MPU9250_ReadReg(MPU6500_WHO_AM_I, &DeviceID);
if(DeviceID != MPU6500_Device_ID)
return ERROR;
return SUCCESS;
}
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;
}
/**
* @brief MPU9250_Check_MPU6500
* @note MPU6500 in MPU9250 ID check
* @param None
* @retval None
*/
uint8_t MPU9250_Check_MPU6500( void )
{
uint8_t DeviceID = 0x00;
/* MPU6500 */
DeviceID = 0x00;
MPU9250_ReadReg(MPU6500_WHO_AM_I, &DeviceID);
rt_kprintf("MPU6500_ID=%d\r\n",DeviceID);
if(DeviceID != MPU6500_Device_ID)
return ERROR;
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));
}
/*******************************************************************************
* 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;
}
/*====================================================================================================*/
uint8_t MPU9250_Check( void )
{
uint8_t DeviceID = ERROR;
DeviceID = MPU9250_ReadReg(MPU6500_WHO_AM_I);
if(DeviceID != MPU6500_Device_ID)
return ERROR;
#ifdef _USE_MAG_AK8963
DeviceID = MPU9250_Mag_ReadReg(AK8963_WIA);
if(DeviceID != AK8963_Device_ID)
return ERROR;
#endif
return SUCCESS;
}
/**
* @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;
}
void MPU9250_GetAccel()
{
int16_t temp;
temp = MPU9250_ReadReg(MPU6500_ACCEL_XOUT_H) << 8;
temp += MPU9250_ReadReg(MPU6500_ACCEL_XOUT_L);
Accel.X = (int32_t)temp;
temp = MPU9250_ReadReg(MPU6500_ACCEL_YOUT_H) << 8;
temp += MPU9250_ReadReg(MPU6500_ACCEL_YOUT_L);
Accel.Y = (int32_t)temp;
temp = MPU9250_ReadReg(MPU6500_ACCEL_ZOUT_H) << 8;
temp += MPU9250_ReadReg(MPU6500_ACCEL_ZOUT_L);
Accel.Z = (int32_t)temp;
}
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--));
}
/*====================================================================================================*/
static u8 MPU9250_Check( void )
{
u8 DeviceID = 0x00;
/* MPU6500 */
MPU9250_ReadReg(MPU6500_WHO_AM_I, &DeviceID);
if(DeviceID != MPU6500_Device_ID)
return ERROR;
// /* AK8963 */
// MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, 0x8C); // Set AK8963_I2C_ADDR = 7'b000_1100
// Delay_1us(10);
// MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_WIA); // Set Write Reg
// MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x81); // Start Read
// Delay_1ms(1);
// MPU9250_ReadReg(MPU6500_EXT_SENS_DATA_00, &DeviceID); // Read Data
// if(DeviceID != AK8963_Device_ID)
// return ERROR;
return SUCCESS;
}
void MPU9250_GetGyro()
{
int16_t temp;
temp = MPU9250_ReadReg(MPU6500_GYRO_XOUT_H) << 8;
temp += MPU9250_ReadReg(MPU6500_GYRO_XOUT_L);
Gyro.X = (int32_t)temp;
temp = MPU9250_ReadReg(MPU6500_GYRO_YOUT_H) << 8;
temp += MPU9250_ReadReg(MPU6500_GYRO_YOUT_L);
Gyro.Y = (int32_t)temp;
temp = MPU9250_ReadReg(MPU6500_GYRO_ZOUT_H) << 8;
temp += MPU9250_ReadReg(MPU6500_GYRO_ZOUT_L);
Gyro.Z = (int32_t)temp;
Gyro.X += Gyro_Offset.X;
Gyro.Y += Gyro_Offset.Y;
Gyro.Z += Gyro_Offset.Z;
}
/*====================================================================================================*/
uint8_t IMU_deviceCheck( IMU_DataTypeDef *pIMU )
{
uint8_t deviceID = ERROR;
if(pIMU->MPU_GyrAcc_Enable == MPU_GyrAcc_ENABLE) {
deviceID = MPU9250_ReadReg(MPU6500_WHO_AM_I);
if(deviceID != MPU6500_Device_ID)
return ERROR;
}
if(pIMU->MPU_Mag_Enable == MPU_Mag_ENABLE) {
deviceID = MPU9250_Mag_ReadReg(AK8963_WIA);
if(deviceID != AK8963_Device_ID)
return ERROR;
}
if(pIMU->LPS_PresTemp_Enable == LPS_PresTemp_ENABLE) {
deviceID = LPS25H_ReadReg(LPS25H_WHO_AM_I);
if(deviceID != LPS25H_Device_ID)
return ERROR;
}
return SUCCESS;
}
void MPU9250_Init()
{
uint32_t Device_ID;
SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_7);
GPIOPinConfigure(GPIO_PA2_SSI0CLK);
GPIOPinConfigure(GPIO_PA3_SSI0FSS);
GPIOPinConfigure(GPIO_PA4_SSI0RX);
GPIOPinConfigure(GPIO_PA5_SSI0TX);
GPIOPinTypeSSI(GPIO_PORTA_BASE, GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5);
GPIOPadConfigSet(GPIO_PORTA_BASE, GPIO_PIN_3, GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);
SSIConfigSetExpClk(SSI0_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_3, SSI_MODE_MASTER, 1000000, 8);
SSIEnable(SSI0_BASE);
//
// Display the example setup on the console.
//
UARTprintf("\nPower up.\n");
// MPU-6500 Device ID check and initial
Device_ID = 0x0;
Device_ID = MPU9250_ReadReg(MPU6500_WHO_AM_I);
if(Device_ID == MPU6500_Device_ID)
{
UARTprintf("MPU6500 Device ID: 0x%X\n--- MPU6500 Initialization done.\n", Device_ID);
}
else
{
OLED_Print(0,0,"MPU6500 ERR");
UARTprintf("MPU6500 Device ID: 0x%X\n--- MPU6500 Initialization error.\n", Device_ID);
while (1);
}
MPU9250_SetParameter();
#ifdef USE_MAG
// AK9875 Device ID check
// if AK9875 ID check check failed with no reason, re-power-up the system.
MPU9250_WriteAuxReg(AK8963_CNTL2, 0x01); //soft reset
MPU9250_Delay_nop(100000);
Device_ID = 0x00;
Device_ID = MPU9250_ReadAuxReg(AK8963_WIA);
if(Device_ID == AK8963_Device_ID)
{
UARTprintf("AK8963 Device ID: 0x%X\n--- AK8963 Initialization done.\n", Device_ID);
LED_Blue_Off();
}
else
{
UARTprintf("AK8963 Device ID: 0x%X\n--- AK8963 Initialization error.\n", Device_ID);
OLED_Print(0,0,"AK8963 Init ERR");
LED_Red_Off();
LED_Green_Off();
}
MPU9250_GetMagAdjValue();
MPU9250_WriteAuxReg(AK8963_CNTL1, 0x16); // 16-bits, Rate 100Hz
//MPU9250_WriteAuxReg(AK8963_CNTL1, 0x2); // 14-bits, Rate 8Hz
Device_ID = MPU9250_ReadAuxReg(AK8963_CNTL1);
#else
UARTprintf("AK8963 is disabled.\n");
#endif
}
