Initial commit

libgy521.c

@@ -0,0 +1,74 @@
+#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;
+}