From db41cd547bfa15ee3f8274962e2022de821c0f76 Mon Sep 17 00:00:00 2001 From: structix Date: Fri, 9 Sep 2016 21:41:50 +0200 Subject: [PATCH] getGyroscopeData, readGyroscopeRange, setGyroscopeRange, extended example --- gy521example.c | 16 ++++- libgy521.c | 189 +++++++++++++++++++++++++++++++++++++++---------- libgy521.h | 18 +++-- 3 files changed, 174 insertions(+), 49 deletions(-) diff --git a/gy521example.c b/gy521example.c index edd39d5..f52e958 100644 --- a/gy521example.c +++ b/gy521example.c @@ -2,13 +2,25 @@ #include #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); } } diff --git a/libgy521.c b/libgy521.c index abb4695..1bfb7cc 100644 --- a/libgy521.c +++ b/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) { - return wiringPiI2CReadReg8(id, MPU6050_GYRO_XOUT_H); +int getAccelerometerData(char axis, int g) { + /** + * 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; + } + + + 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 getGyroY(void) { - return wiringPiI2CReadReg8(id, MPU6050_GYRO_YOUT_H); +void setGyroscopeRange(int range) { + //Same procedure as in setAccelerometerRange() + wiringPiI2CWriteReg8(id, GYRO_CONFIG, 0x00); + wiringPiI2CWriteReg8(id, GYRO_CONFIG, range); } -int getGyroZ(void) { - return wiringPiI2CReadReg8(id, MPU6050_GYRO_ZOUT_H); +int readGyroscopeRange(int raw) { + 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 + } + } + + return range; } -int getAccelX(void) { - return wiringPiI2CReadReg8(id, MPU6050_ACCEL_XOUT_H); -} -int getAccelY(void) { - 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 y = getAccelY(); - int z = getAccelZ(); + int x = getAccelerometerData('x', 0); + int y = getAccelerometerData('y', 0); + 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 y = getAccelY(); - int z = getAccelZ(); + int x = getAccelerometerData('x', 1); + int y = getAccelerometerData('y', 1); + 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 y = getAccelY(); - int z = getAccelZ(); + int x = getAccelerometerData('x', 1); + int y = getAccelerometerData('y', 1); + int z = getAccelerometerData('z', 1); return atan((sqrt(pow(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI; } diff --git a/libgy521.h b/libgy521.h index c88e93a..ecdccb7 100644 --- a/libgy521.h +++ b/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);