void initMPU6500(mpu6500_t *mpu6500) {
uint8_t i2cBuffer[5]; // Buffer for I2C data
i2cBuffer[0] = i2cRead(MPU6500_ADDRESS, MPU6500_WHO_AM_I);
if (i2cBuffer[0] == MPU6500_WHO_AM_I_ID) { // Read "WHO_AM_I" register
#if UART_DEBUG
UARTprintf("MPU-6500 found\n");
#endif
} else if (i2cBuffer[0] == MPU9250_WHO_AM_I_ID) {
#if UART_DEBUG
UARTprintf("MPU-9250 found\n");
#endif
} else {
#if UART_DEBUG
UARTprintf("Could not find MPU-6500 or MPU-9250: %2X\n", i2cBuffer[0]);
#endif
while (1);
}
i2cWrite(MPU6500_ADDRESS, MPU6500_PWR_MGMT_1, (1 << 7)); // Reset device, this resets all internal registers to their default values
delay(100);
while (i2cRead(MPU6500_ADDRESS, MPU6500_PWR_MGMT_1) & (1 << 7)) {
// Wait for the bit to clear
};
delay(100);
i2cWrite(MPU6500_ADDRESS, MPU6500_PWR_MGMT_1, (1 << 3) | (1 << 0)); // Disable sleep mode, disable temperature sensor and use PLL as clock reference
i2cBuffer[0] = 0; // Set the sample rate to 1kHz - 1kHz/(1+0) = 1kHz
i2cBuffer[1] = 0x03; // Disable FSYNC and set 41 Hz Gyro filtering, 1 KHz sampling
i2cBuffer[2] = 3 << 3; // Set Gyro Full Scale Range to +-2000deg/s
i2cBuffer[3] = 2 << 3; // Set Accelerometer Full Scale Range to +-8g
i2cBuffer[4] = 0x03; // 41 Hz Acc filtering
i2cWriteData(MPU6500_ADDRESS, MPU6500_SMPLRT_DIV, i2cBuffer, 5); // Write to all five registers at once
// Set accelerometer and gyroscope scale factor from datasheet
mpu6500->gyroScaleFactor = MPU6500_GYRO_SCALE_FACTOR_2000;
mpu6500->accScaleFactor = MPU6500_ACC_SCALE_FACTOR_8;
/* Enable Raw Data Ready Interrupt on INT pin and enable bypass/passthrough mode */
i2cBuffer[0] = (1 << 5) | (1 << 4) | (1 << 1); // Enable LATCH_INT_EN, INT_ANYRD_2CLEAR and BYPASS_EN
// When this bit is equal to 1, the INT pin is held high until the interrupt is cleared
// When this bit is equal to 1, interrupt status is cleared if any read operation is performed
// When asserted, the I2C_MASTER interface pins (ES_CL and ES_DA) will go into 'bypass mode' when the I2C master interface is disabled
i2cBuffer[1] = (1 << 0); // Enable RAW_RDY_EN - When set to 1, Enable Raw Sensor Data Ready interrupt to propagate to interrupt pin
i2cWriteData(MPU6500_ADDRESS, MPU6500_INT_PIN_CFG, i2cBuffer, 2); // Write to both registers at once
// Set INT input pin
SysCtlPeripheralEnable(GPIO_MPU_INT_PERIPH); // Enable GPIO peripheral
SysCtlDelay(2); // Insert a few cycles after enabling the peripheral to allow the clock to be fully activated
GPIOPinTypeGPIOInput(GPIO_MPU_INT_BASE, GPIO_MPU_INT_PIN); // Set as input
delay(100); // Wait for sensor to stabilize
while (calibrateMPU6500Gyro()) { // Get gyro zero values
// Loop until calibration is successful
}
}
/*---------------------------------------------------------------------------*/
i2cStatus
i2cReadData( char address, char *buf, int buflen, char *rxbuf, int replylen )
{
if( buflen > MAX_I2C_MESSAGE )
return( kMessageTooLong );
// Send message (if necessary)
int timeout = DEFAULT_I2C_TIMEOUT;
if( i2cWriteData( address, buf, buflen, replylen ) == kMessageSuccess ) {
// Now wait for reply data
// If the sensor is disconnected this will block for several mS
while( nI2CBytesReady < replylen && timeout != 0 ) {
timeout--;
wait1Msec(1);
}
// Reply arrived ?
if(timeout > 0)
readI2CReply( rxbuf, replylen );
}
// Return status
if( timeout > 0 && nI2CStatus == i2cStatusDone )
return( kMessageSuccess );
else
return( kMessageError );
}
