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

getGyroscopeData, readGyroscopeRange, setGyroscopeRange, extended example

Browse Source
This commit is contained in:
structix
2016-09-09 21:41:50 +02:00
parent 06753bc2b1
commit db41cd547b
3 changed files with 174 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
libgy521.c
View File

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