getGyroscopeData, readGyroscopeRange, setGyroscopeRange, extended example

gy521example.c

@@ -2,13 +2,25 @@
 #include <wiringPi.h>
 #include "libgy521.h"
 
+
+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 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 Range: raw: %i, int: %i\n", readGyroscopeRange(1), readGyroscopeRange(0));
+    printf("---------------------------------------------\n");
+}
+
 int main(void) {
     wiringPiSetup();
     
     setupgy521();
-    
+    printall();
     while (1) {
-        printf("AngleX = %f, AngleY = %f, AngleZ = %f, Temperature = %f\n", getAngleX(), getAngleY(), getAngleZ(), getTemp());
+        //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);
     }
 }

libgy521.c

@@ -7,21 +7,8 @@
 
 //Constants
 
-//######## Obsolete ########//
-
-#define MPU6050_GYRO_XOUT_H 0x43 //R
-#define MPU6050_GYRO_YOUT_H 0x45 //R
-#define MPU6050_GYRO_ZOUT_H 0x47 //R
-
-#define MPU6050_ACCEL_XOUT_H 0x3B //R
-#define MPU6050_ACCEL_YOUT_H 0x3D //R
-#define MPU6050_ACCEL_ZOUT_H 0x3F //R
-
-#define MPU6050_PWR_MGMT_1 0x6B //R/W
 #define MPU6050_I2C_ADDRESS 0x68 //I2C
 
-//######## End Obsolete ########//
-
 #define GRAVITIY_MS2                9.80665
 
 // Scale Modifiers
@@ -81,6 +68,7 @@ int setupgy521(void) {
     if ((id = wiringPiI2CSetup(MPU6050_I2C_ADDRESS)) != -1) {
         wiringPiI2CReadReg8(id, PWR_MGMT_1);
         wiringPiI2CWriteReg16(id, PWR_MGMT_1, 0);
+        //setAccelerometerRange(ACCEL_RANGE_2G);
         return EXIT_SUCCESS;
     } else {
         return EXIT_FAILURE;
@@ -100,7 +88,7 @@ float getTemp(void) {
      * Formula given in
      * MPU-6050 Register Map and Descriptions revision 4.2, page 30
      */
-    return (read_i2c_word16(TEMP_OUT0) / 340.0) + 36.53; //16 might be bullshit
+    return (read_i2c_word(TEMP_OUT0) / 340.0) + 36.53; 
 }
 
 void setAccelerometerRange(int range) {
@@ -114,8 +102,10 @@ int readAccelerometerRange(int raw) {
     int rawData = wiringPiI2CReadReg8(id, ACCEL_CONFIG);
     int range;
     if (raw != 0) {
+        //True
         range = rawData;
     } else {
+        //False
         switch (rawData) {
             case ACCEL_RANGE_2G:
                 range = 2;
@@ -138,47 +128,172 @@ int readAccelerometerRange(int raw) {
     return range;
 }
 
-int getGyroX(void) {
+int getAccelerometerData(char axis, int g) {
-    return wiringPiI2CReadReg8(id, MPU6050_GYRO_XOUT_H);
+    /**
+     * Returns the Accelerometer data of the specified axis
+     * in the desired unit:
+     * g: g = 1
+     * ms^2: g = 0
+     */
+    
+    int data, scale, range;
+    
+    
+    switch (axis) {
+        case 'x':
+        case 'X':
+            data = read_i2c_word(ACCEL_XOUT0);
+            break;
+        case 'y':
+        case 'Y':
+            data = read_i2c_word(ACCEL_YOUT0);
+            break;
+        case 'z':
+        case 'Z':
+            data = read_i2c_word(ACCEL_ZOUT0);
+            break;
+        default:
+            printf("ERROR: Invalid character %c, using x-axis\n", axis);
+            data = read_i2c_word(ACCEL_XOUT0);
+            break;
     }
     
-int getGyroY(void) {
+    
-    return wiringPiI2CReadReg8(id, MPU6050_GYRO_YOUT_H);
+    range = readAccelerometerRange(1);
+    switch (range) {
+        case ACCEL_RANGE_2G:
+            scale = ACCEL_SCALE_MODIFIER_2G;
+            break;
+        case ACCEL_RANGE_4G:
+            scale = ACCEL_SCALE_MODIFIER_4G;
+            break;
+        case ACCEL_RANGE_8G:
+            scale = ACCEL_SCALE_MODIFIER_8G;
+            break;
+        case ACCEL_RANGE_16G:
+            scale = ACCEL_SCALE_MODIFIER_16G;
+            break;
+        default:
+            printf("ERROR: Unknown range %i, set to default value: %i\n", range, ACCEL_RANGE_2G);
+            scale = ACCEL_SCALE_MODIFIER_2G;
+            break; //Mandatory shit
+    }
+    data /= scale;
+    
+    if (g == 0) {
+        //false --> convert to ms^2
+        data *= GRAVITIY_MS2;
+    }
+    return data;
 }
 
-int getGyroZ(void) {
+void setGyroscopeRange(int range) {
-    return wiringPiI2CReadReg8(id, MPU6050_GYRO_ZOUT_H);
+    //Same procedure as in setAccelerometerRange()
+    wiringPiI2CWriteReg8(id, GYRO_CONFIG, 0x00);
+    wiringPiI2CWriteReg8(id, GYRO_CONFIG, range);
 }
 
-int getAccelX(void) {
+int readGyroscopeRange(int raw) {
-    return wiringPiI2CReadReg8(id, MPU6050_ACCEL_XOUT_H);
+    int rawData = wiringPiI2CReadReg8(id, GYRO_CONFIG);
+    int range;
+    if (raw != 0) {
+        //True
+        range = rawData;
+    } else {
+        //False
+        switch (rawData) {
+            case GYRO_RANGE_250DEG:
+                range = 250;
+                break;
+            case GYRO_RANGE_500DEG:
+                range = 500;
+                break;
+            case GYRO_RANGE_1000DEG:
+                range = 1000;
+                break;
+            case GYRO_RANGE_2000DEG:
+                range = 2000;
+                break;
+            default:
+                range = -1; //Error
+                break; //Pretty good manner :D
+        }
     }
     
-int getAccelY(void) {
+    return range;
-    return wiringPiI2CReadReg8(id, MPU6050_ACCEL_YOUT_H);
 }
 
-int getAccelZ(void) {
+
-    return wiringPiI2CReadReg8(id, MPU6050_ACCEL_ZOUT_H);
+
+int getGyroscopeData(char axis) {
+    /**
+     * Returns the Accelerometer data of the specified axis
+     */
+    
+    int data, scale, range;
+    
+    
+    switch (axis) {
+        case 'x':
+        case 'X':
+            data = read_i2c_word(GYRO_XOUT0);
+            break;
+        case 'y':
+        case 'Y':
+            data = read_i2c_word(GYRO_YOUT0);
+            break;
+        case 'z':
+        case 'Z':
+            data = read_i2c_word(GYRO_ZOUT0);
+            break;
+        default:
+            printf("ERROR: Invalid character %c, using x-axis\n", axis);
+            data = read_i2c_word(GYRO_XOUT0);
+            break;
+    }
+    
+    
+    range = readGyroscopeRange(1);
+    switch (range) {
+        case GYRO_RANGE_250DEG:
+            scale = GYRO_SCALE_MODIFIER_250DEG;
+            break;
+        case GYRO_RANGE_500DEG:
+            scale = GYRO_SCALE_MODIFIER_500DEG;
+            break;
+        case GYRO_RANGE_1000DEG:
+            scale = GYRO_SCALE_MODIFIER_1000DEG;
+            break;
+        case GYRO_RANGE_2000DEG:
+            scale = GYRO_RANGE_2000DEG;
+            break;
+        default:
+            printf("ERROR: Unknown range %i, set to default value: %fd\n", range, GYRO_SCALE_MODIFIER_250DEG);
+            scale = GYRO_SCALE_MODIFIER_250DEG;
+            break; //Mandatory shit
+    }
+    
+    data /= scale;
+    return data;
 }
 
 float getAngleX(void) {
-    int x = getAccelX();
+    int x = getAccelerometerData('x', 0);
-    int y = getAccelY();
+    int y = getAccelerometerData('y', 0);
-    int z = getAccelZ();
+    int z = getAccelerometerData('z', 0);
     return atan(x / (sqrt(pow(y, 2) + pow(z, 2)))) * 180.0 / M_PI;
 }
 
 float getAngleY(void) {
-    int x = getAccelX();
+    int x = getAccelerometerData('x', 1);
-    int y = getAccelY();
+    int y = getAccelerometerData('y', 1);
-    int z = getAccelZ();
+    int z = getAccelerometerData('z', 1);
     return atan(y / (sqrt(pow(x, 2) + pow(z, 2)))) * 180.0 / M_PI;
 }
 
 float getAngleZ(void) {
-    int x = getAccelX();
+    int x = getAccelerometerData('x', 1);
-    int y = getAccelY();
+    int y = getAccelerometerData('y', 1);
-    int z = getAccelZ();
+    int z = getAccelerometerData('z', 1);
     return atan((sqrt(pow(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI;
 }

libgy521.h

@@ -1,15 +1,13 @@
 int setupgy521(void);
-int getGyroX(void);
-int getGyroY(void);
-int getGyroZ(void);
-int getAccelX(void);
-int getAccelY(void);
-int getAccelZ(void);
-float getAngleX(void);
-float getAngleY(void);
-float getAngleZ(void);
 int read_i2c_word(int reg);
+int read_i2c_word16(int reg);
 float getTemp(void);
 void setAccelerometerRange(int range);
 int readAccelerometerRange(int raw);
-int read_i2c_word16(int reg);
+int getAccelerometerData(char axis, int g);
+void setGyroscopeRange(int range);
+int readGyroscopeRange(int raw);
+int getGyroscopeData(char axis);
+float getAngleX(void);
+float getAngleY(void);
+float getAngleZ(void);