IMU problem - I2C

Projects Programming
4 
 I2C_write(GYRO_ADDRESS, 0x39, 0b00000000);

I don't see a register 0x39 in the data sheet. http://www.st.com/content/ccc/resource/technical/document/datasheet/43/37/e3/06/b0/bf/48/bd/DM00036465.pdf/files/DM00036465.pdf/jcr:content/translations/en.DM00036465.pdf

7.10 OUT_X_L (28h), OUT_X_H (29h)
X-axis angular rate data. The value is expressed as two’s complement.

I think you have your high and low bytes switched around when you read an recombine them.

inline void getRotation()
{
    double temp = I2C_read(GYRO_ADDRESS, 0x28, 2);
    temp *= GYRO_SENSIBILITY500;
    Serial.print(temp);
    Serial.println(" degrees/second");
}

long I2C_read(byte slaveAddress, byte registerAddress, byte numberOfBytes)
{
    Wire.beginTransmission(slaveAddress);
    Wire.write(registerAddress);
    if(Wire.endTransmission()!=0)
      Serial.println("Reading Error!");
    Wire.requestFrom(slaveAddress, numberOfBytes);

    while(Wire.available() < numberOfBytes);
    long temp = 0;
    for(int i=1; i<=numberOfBytes; i++)
    {
      temp += (long)Wire.read()<<(8*(numberOfBytes - i));
    }
    return temp;
}