75 lines
1.6 KiB
C
75 lines
1.6 KiB
C
#include <wiringPi.h>
|
|
#include <math.h>
|
|
|
|
#include <libgy521.h>
|
|
|
|
//Constants
|
|
|
|
#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
|
|
|
|
int id;
|
|
|
|
int setupgy521(void) {
|
|
if (id = wiringPiI2CSetup(MPU6050_I2C_ADDRESS) != -1) {
|
|
wiringPiI2CReadReg8(id, MPU6050_PWR_MGMT_1);
|
|
wiringPiI2CWriteReg16(id, MPU6050_PWR_MGMT_1, 0);
|
|
return EXIT_SUCCESS;
|
|
} else {
|
|
return EXIT_FAILURE
|
|
}
|
|
}
|
|
|
|
int getGyroX(void) {
|
|
return wiringPiI2CReadReg8(id, MPU6050_GYRO_XOUT_H);
|
|
}
|
|
|
|
int getGyroY(void) {
|
|
return wiringPiI2CReadReg8(id, MPU6050_GYRO_YOUT_H);
|
|
}
|
|
|
|
int getGyroZ(void) {
|
|
return wiringPiI2CReadReg8(id, MPU6050_GYRO_ZOUT_H);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
float getAngleX(void) {
|
|
int x = getAccelX();
|
|
int y = getAccelY();
|
|
int z = getAccelZ();
|
|
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();
|
|
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();
|
|
return atan((sqrt(po(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI;
|
|
}
|