#include #include #include #include #include "libgy521.h" //Constants #define MPU6050_I2C_ADDRESS 0x68 //I2C #define GRAVITIY_MS2 9.80665 // Scale Modifiers #define ACCEL_SCALE_MODIFIER_2G 16384.0 #define ACCEL_SCALE_MODIFIER_4G 8192.0 #define ACCEL_SCALE_MODIFIER_8G 4096.0 #define ACCEL_SCALE_MODIFIER_16G 2048.0 #define GYRO_SCALE_MODIFIER_250DEG 131.0 #define GYRO_SCALE_MODIFIER_500DEG 65.5 #define GYRO_SCALE_MODIFIER_1000DEG 32.8 #define GYRO_SCALE_MODIFIER_2000DEG 16.4 // Pre-defined ranges #define ACCEL_RANGE_2G 0x00 #define ACCEL_RANGE_4G 0x08 #define ACCEL_RANGE_8G 0x10 #define ACCEL_RANGE_16G 0x18 #define GYRO_RANGE_250DEG 0x00 #define GYRO_RANGE_500DEG 0x08 #define GYRO_RANGE_1000DEG 0x10 #define GYRO_RANGE_2000DEG 0x18 // MPU-6050 Registers #define PWR_MGMT_1 0x6B //R/W #define PWR_MGMT_2 0x6C #define SELF_TEST_X 0x0D #define SELF_TEST_Y 0x0E #define SELF_TEST_Z 0x0F #define SELF_TEST_A 0x10 #define ACCEL_XOUT0 0x3B #define ACCEL_XOUT1 0x3C #define ACCEL_YOUT0 0x3D #define ACCEL_YOUT1 0x3E #define ACCEL_ZOUT0 0x3F #define ACCEL_ZOUT1 0x40 #define TEMP_OUT0 0x41 #define TEMP_OUT1 0x42 #define GYRO_XOUT0 0x43 //R #define GYRO_XOUT1 0x44 #define GYRO_YOUT0 0x45 //R #define GYRO_YOUT1 0x46 #define GYRO_ZOUT0 0x47 //R #define GYRO_ZOUT1 0x48 #define ACCEL_CONFIG 0x1C #define GYRO_CONFIG 0x1B int id; 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; } } int read_i2c_word(int reg) { return wiringPiI2CReadReg8(id, reg); } 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_combined(TEMP_OUT0) / 340.0) + 36.53; } void setAccelerometerRange(int range) { //First set data to 0x00 wiringPiI2CWriteReg8(id, ACCEL_CONFIG, 0x00); //Then write the correct value to the register wiringPiI2CWriteReg8(id, ACCEL_CONFIG, range); } 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; break; case ACCEL_RANGE_4G: range = 4; break; case ACCEL_RANGE_8G: range = 8; break; case ACCEL_RANGE_16G: range = 16; break; default: range = -1; //Error break; //Good manner ;) } } return range; } float 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, range; float result, scale; switch (axis) { case 'x': case 'X': data = read_i2c_word_combined(ACCEL_XOUT0); break; case 'y': case 'Y': data = read_i2c_word_combined(ACCEL_YOUT0); break; case 'z': case 'Z': data = read_i2c_word_combined(ACCEL_ZOUT0); break; default: printf("ERROR: Invalid character %c, using x-axis\n", axis); data = read_i2c_word_combined(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 } result = data / scale; if (g == 0) { //false --> convert to ms^2 result = data * GRAVITIY_MS2; } return result; } void setGyroscopeRange(int range) { //Same procedure as in setAccelerometerRange() wiringPiI2CWriteReg8(id, GYRO_CONFIG, 0x00); wiringPiI2CWriteReg8(id, GYRO_CONFIG, range); } 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; } float getGyroscopeData(char axis) { /** * Returns the Accelerometer data of the specified axis */ int data, range; float result, scale; switch (axis) { case 'x': case 'X': data = read_i2c_word_combined(GYRO_XOUT0); break; case 'y': case 'Y': data = read_i2c_word_combined(GYRO_YOUT0); break; case 'z': case 'Z': data = read_i2c_word_combined(GYRO_ZOUT0); break; default: printf("ERROR: Invalid character %c, using x-axis\n", axis); data = read_i2c_word_combined(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_SCALE_MODIFIER_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 } result = data / scale; return result; } float getAngleX(void) { 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) { 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) { 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; }