Major bugfixes, added read_i2c_word_combined

gy521example.c

@@ -5,10 +5,11 @@
 
 void printall(void) {
     printf("---------------------------------------------\n");
-    printf("Accel: x=%i, y=%i, z=%i\n", getAccelerometerData('s', 0), getAccelerometerData('y', 0), getAccelerometerData('z', 0));
+    printf("Accel: x=%f, y=%f, z=%f\n", getAccelerometerData('x', 1), getAccelerometerData('y', 1), getAccelerometerData('z', 1));
     printf("Accel Range: raw: %i, int: %i\n", readAccelerometerRange(1), readAccelerometerRange(0));
-    printf("Gyro: x=%i, y=%i, z=%i\n", getGyroscopeData('x'), getGyroscopeData('y'), getGyroscopeData('z'));
+    printf("Gyro: x=%f, y=%f, z=%f\n", getGyroscopeData('x'), getGyroscopeData('y'), getGyroscopeData('z'));
     printf("Gyro Range: raw: %i, int: %i\n", readGyroscopeRange(1), readGyroscopeRange(0));
+    printf("Temp: %f\n", getTemp());
     printf("---------------------------------------------\n");
 }
 
@@ -21,6 +22,6 @@ int main(void) {
         //printf("AngleX = %f, AngleY = %f, AngleZ = %f, Temperature = %f\n", getAngleX(), getAngleY(), getAngleZ(), getTemp());
         //printf("Gyro: x=%i, y=%i, z=%i\n", getGyroscopeData('x'), getGyroscopeData('y'), getGyroscopeData('z'));
         printall();
-        delay(100);
+        delay(500);
     }
 }

libgy521.c

@@ -83,12 +83,31 @@ int read_i2c_word16(int reg) {
     return wiringPiI2CReadReg16(id, reg);
 }
 
+int read_i2c_word_combined(int reg) {
+    int high = read_i2c_word(reg);
+    int low = read_i2c_word(reg + 1);
+    
+    int value = (high << 8) + low;
+    
+    if (value >= 0x8000) {
+        return -((65535 - value) + 1);
+    } else {
+        return value;
+    }        
+}
+        
+
+        
+
+        
+
+
 float getTemp(void) {
     /**
      * Formula given in
      * MPU-6050 Register Map and Descriptions revision 4.2, page 30
      */
-    return (read_i2c_word(TEMP_OUT0) / 340.0) + 36.53; 
+    return (read_i2c_word_combined(TEMP_OUT0) / 340.0) + 36.53; 
 }
 
 void setAccelerometerRange(int range) {
@@ -128,7 +147,7 @@ int readAccelerometerRange(int raw) {
     return range;
 }
 
-int getAccelerometerData(char axis, int g) {
+float getAccelerometerData(char axis, int g) {
     /**
      * Returns the Accelerometer data of the specified axis
      * in the desired unit:
@@ -136,25 +155,25 @@ int getAccelerometerData(char axis, int g) {
      * ms^2: g = 0
      */
     
-    int data, scale, range;
+    int data, range;
-    
+    float result, scale;
     
     switch (axis) {
         case 'x':
         case 'X':
-            data = read_i2c_word(ACCEL_XOUT0);
+            data = read_i2c_word_combined(ACCEL_XOUT0);
             break;
         case 'y':
         case 'Y':
-            data = read_i2c_word(ACCEL_YOUT0);
+            data = read_i2c_word_combined(ACCEL_YOUT0);
             break;
         case 'z':
         case 'Z':
-            data = read_i2c_word(ACCEL_ZOUT0);
+            data = read_i2c_word_combined(ACCEL_ZOUT0);
             break;
         default:
             printf("ERROR: Invalid character %c, using x-axis\n", axis);
-            data = read_i2c_word(ACCEL_XOUT0);
+            data = read_i2c_word_combined(ACCEL_XOUT0);
             break;
     }
     
@@ -178,13 +197,13 @@ int getAccelerometerData(char axis, int g) {
             scale = ACCEL_SCALE_MODIFIER_2G;
             break; //Mandatory shit
     }
-    data /= scale;
+    result = data / scale;
     
     if (g == 0) {
         //false --> convert to ms^2
-        data *= GRAVITIY_MS2;
+        result = data * GRAVITIY_MS2;
     }
-    return data;
+    return result;
 }
 
 void setGyroscopeRange(int range) {
@@ -225,30 +244,30 @@ int readGyroscopeRange(int raw) {
 
 
 
-int getGyroscopeData(char axis) {
+float getGyroscopeData(char axis) {
     /**
      * Returns the Accelerometer data of the specified axis
      */
     
-    int data, scale, range;
+    int data, range;
-    
+    float result, scale;
     
     switch (axis) {
         case 'x':
         case 'X':
-            data = read_i2c_word(GYRO_XOUT0);
+            data = read_i2c_word_combined(GYRO_XOUT0);
             break;
         case 'y':
         case 'Y':
-            data = read_i2c_word(GYRO_YOUT0);
+            data = read_i2c_word_combined(GYRO_YOUT0);
             break;
         case 'z':
         case 'Z':
-            data = read_i2c_word(GYRO_ZOUT0);
+            data = read_i2c_word_combined(GYRO_ZOUT0);
             break;
         default:
             printf("ERROR: Invalid character %c, using x-axis\n", axis);
-            data = read_i2c_word(GYRO_XOUT0);
+            data = read_i2c_word_combined(GYRO_XOUT0);
             break;
     }
     
@@ -265,7 +284,7 @@ int getGyroscopeData(char axis) {
             scale = GYRO_SCALE_MODIFIER_1000DEG;
             break;
         case GYRO_RANGE_2000DEG:
-            scale = GYRO_RANGE_2000DEG;
+            scale = GYRO_SCALE_MODIFIER_2000DEG;
             break;
         default:
             printf("ERROR: Unknown range %i, set to default value: %fd\n", range, GYRO_SCALE_MODIFIER_250DEG);
@@ -273,27 +292,27 @@ int getGyroscopeData(char axis) {
             break; //Mandatory shit
     }
     
-    data /= scale;
+    result = data / scale;
-    return data;
+    return result;
 }
 
 float getAngleX(void) {
-    int x = getAccelerometerData('x', 0);
+    float x = getAccelerometerData('x', 0);
-    int y = getAccelerometerData('y', 0);
+    float y = getAccelerometerData('y', 0);
-    int z = getAccelerometerData('z', 0);
+    float z = getAccelerometerData('z', 0);
     return atan(x / (sqrt(pow(y, 2) + pow(z, 2)))) * 180.0 / M_PI;
 }
 
 float getAngleY(void) {
-    int x = getAccelerometerData('x', 1);
+    float x = getAccelerometerData('x', 0);
-    int y = getAccelerometerData('y', 1);
+    float y = getAccelerometerData('y', 0);
-    int z = getAccelerometerData('z', 1);
+    float z = getAccelerometerData('z', 0);
     return atan(y / (sqrt(pow(x, 2) + pow(z, 2)))) * 180.0 / M_PI;
 }
 
 float getAngleZ(void) {
-    int x = getAccelerometerData('x', 1);
+    float x = getAccelerometerData('x', 0);
-    int y = getAccelerometerData('y', 1);
+    float y = getAccelerometerData('y', 0);
-    int z = getAccelerometerData('z', 1);
+    float z = getAccelerometerData('z', 0);
     return atan((sqrt(pow(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI;
 }

libgy521.h

@@ -1,13 +1,14 @@
 extern int setupgy521(void);
 extern int read_i2c_word(int reg);
 extern int read_i2c_word16(int reg);
+extern int read_i2c_word_combined(int reg);
 extern float getTemp(void);
 extern void setAccelerometerRange(int range);
 extern int readAccelerometerRange(int raw);
-extern int getAccelerometerData(char axis, int g);
+extern float getAccelerometerData(char axis, int g);
 extern void setGyroscopeRange(int range);
 extern int readGyroscopeRange(int raw);
-extern int getGyroscopeData(char axis);
+extern float getGyroscopeData(char axis);
 extern float getAngleX(void);
 extern float getAngleY(void);
 extern float getAngleZ(void);