320 lines
8.4 KiB
C
320 lines
8.4 KiB
C
#include <wiringPiI2C.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#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;
|
|
}
|
|
|
|
float getAccelerometerAverage(char axis, int g, unsigned int measurements) {
|
|
int i;
|
|
float average = 0.0;
|
|
for (i = 0; i < measurements; i++) {
|
|
average += getAccelerometerData(axis, g);
|
|
}
|
|
return average / (float) measurements;
|
|
}
|
|
|
|
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;
|
|
}
|