structix/libgy521
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Major bugfixes, added read_i2c_word_combined

Browse Source
This commit is contained in:
structix
2016-09-10 15:25:15 +02:00
parent 457cc459cf
commit 3a7ab407c3
3 changed files with 56 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
libgy521.c
View File

@@ -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;
}