void MPU60xx_GetData(MPU6050_Data *sensorData)
{
uint8_t data[14]; // TODO: Set to proper byte size dependent on what sampling is being done.
// Read how many bytes have been loaded into the FIFO
I2C_ReadFromReg_Burst(MPU60XX_ADDRESS, RA_FIFO_COUNT_H, data, 2);
uint16_t bytesInBuffer = (data[0] << 8) | data[1];
// If there's data available, read a single timestamp
if (bytesInBuffer >= sizeof(data)) {
I2C_ReadFromReg_Burst(MPU60XX_ADDRESS, RA_FIFO_R_W, data, 14);
// Now fit this data into our nice pretty struct
sensorData->accelX = data[1];
sensorData->accelX |= data[0] << 8;
sensorData->accelY = data[3];
sensorData->accelY |= data[2] << 8;
sensorData->accelZ = data[5];
sensorData->accelZ |= data[4] << 8;
sensorData->temp = data[7];
sensorData->temp |= data[6] << 8;
sensorData->gyroX = data[9];
sensorData->gyroX |= data[8] << 8;
sensorData->gyroY = data[11];
sensorData->gyroY |= data[10] << 8;
sensorData->gyroZ = data[13];
sensorData->gyroZ |= data[12] << 8;
}
}
uint8_t I2C_ReadFromReg(uint8_t address, uint8_t deviceRegister)
{
uint8_t tempData;
I2C_ReadFromReg_Burst(address, deviceRegister, &tempData, 1);
return tempData;
}
