From 3a7ab407c3a0854077d9d9485ef9e23d9e007432 Mon Sep 17 00:00:00 2001 From: structix Date: Sat, 10 Sep 2016 15:25:15 +0200 Subject: [PATCH] Major bugfixes, added read_i2c_word_combined --- gy521example.c | 7 +++-- libgy521.c | 79 +++++++++++++++++++++++++++++++------------------- libgy521.h | 5 ++-- 3 files changed, 56 insertions(+), 35 deletions(-) diff --git a/gy521example.c b/gy521example.c index f52e958..2bb7321 100644 --- a/gy521example.c +++ b/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); } } diff --git a/libgy521.c b/libgy521.c index 1bfb7cc..5975961 100644 --- a/libgy521.c +++ b/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; - return data; + result = data / scale; + return result; } float getAngleX(void) { - int x = getAccelerometerData('x', 0); - int y = getAccelerometerData('y', 0); - int z = getAccelerometerData('z', 0); + float x = getAccelerometerData('x', 0); + float y = getAccelerometerData('y', 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); - int y = getAccelerometerData('y', 1); - int z = getAccelerometerData('z', 1); + float x = getAccelerometerData('x', 0); + float y = getAccelerometerData('y', 0); + 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); - int y = getAccelerometerData('y', 1); - int z = getAccelerometerData('z', 1); + float x = getAccelerometerData('x', 0); + float y = getAccelerometerData('y', 0); + float z = getAccelerometerData('z', 0); return atan((sqrt(pow(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI; } diff --git a/libgy521.h b/libgy521.h index ef356d8..c1cc485 100644 --- a/libgy521.h +++ b/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);