unsigned short MPU9250_DMP::fifoAvailable(void)
{
unsigned char fifoH, fifoL;
if (mpu_read_reg(MPU9250_FIFO_COUNTH, &fifoH) != INV_SUCCESS)
return 0;
if (mpu_read_reg(MPU9250_FIFO_COUNTL, &fifoL) != INV_SUCCESS)
return 0;
return (fifoH << 8 ) | fifoL;
}
uint8_t mpu_device_init(void)
{
ENABLE_IST;
// Reset the internal registers
mpu_write_reg(MPU6500_PWR_MGMT_1, 0x80);
MPU_INIT_DELAY(100);
// Reset gyro/accel/temp digital signal path
mpu_write_reg(MPU6500_SIGNAL_PATH_RESET, 0x07);
MPU_INIT_DELAY(100);
if (MPU6500_ID != mpu_read_reg(MPU6500_WHO_AM_I))
return 1;
uint8_t MPU6500_Init_Data[7][2] = {
{ MPU6500_PWR_MGMT_1, 0x03 }, // Auto selects Clock Source
{ MPU6500_PWR_MGMT_2, 0x00 }, // all enable
{ MPU6500_CONFIG, 0x00 }, // gyro bandwidth 0x00:250Hz 0x04:20Hz
{ MPU6500_GYRO_CONFIG, 0x10 }, // gyro range 0x10:+-1000dps 0x18:+-2000dps
{ MPU6500_ACCEL_CONFIG, 0x10 }, // acc range 0x10:+-8G
{ MPU6500_ACCEL_CONFIG_2, 0x00 }, // acc bandwidth 0x00:250Hz 0x04:20Hz
{ MPU6500_USER_CTRL, 0x20 }, // Enable the I2C Master I/F module
// pins ES_DA and ES_SCL are isolated from
// pins SDA/SDI and SCL/SCLK.
};
uint8_t i = 0;
for (i = 0; i < 7; i++)
{
mpu_write_reg(MPU6500_Init_Data[i][0], MPU6500_Init_Data[i][1]);
MPU_INIT_DELAY(1);
}
ist8310_init();
mpu_offset_cal();
return 0;
}
bool MPU9250_DMP::dataReady()
{
unsigned char intStatusReg;
if (mpu_read_reg(MPU9250_INT_STATUS, &intStatusReg) == INV_SUCCESS)
{
return (intStatusReg & (1<<INT_STATUS_RAW_DATA_RDY_INT));
}
return false;
}
static uint8_t ist_reg_read_by_mpu(uint8_t addr)
{
uint8_t retval;
mpu_write_reg(MPU6500_I2C_SLV4_REG, addr);
MPU_INIT_DELAY(10);
mpu_write_reg(MPU6500_I2C_SLV4_CTRL, 0x80);
MPU_INIT_DELAY(10);
retval = mpu_read_reg(MPU6500_I2C_SLV4_DI);
//turn off slave4 after read
mpu_write_reg(MPU6500_I2C_SLV4_CTRL, 0x00);
MPU_INIT_DELAY(10);
return retval;
}