Affordable Smart Wheelchair / IMU6050Ver11

a

IMU6050/MPU6050.cpp@17:f2e2692762ac, 2019-07-09 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 09 23:36:51 2019 +0000
Revision:
17:f2e2692762ac
Parent:
15:b7cac36c4cce 
gg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jvfausto 15:b7cac36c4cce 1 /**
jvfausto 15:b7cac36c4cce 2 * Includes
jvfausto 15:b7cac36c4cce 3 */
jvfausto 15:b7cac36c4cce 4 #include "MPU6050.h"
jvfausto 15:b7cac36c4cce 5
jvfausto 15:b7cac36c4cce 6 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
jvfausto 15:b7cac36c4cce 7 this->setSleepMode(false);
jvfausto 15:b7cac36c4cce 8
jvfausto 15:b7cac36c4cce 9 //Initializations:
jvfausto 15:b7cac36c4cce 10 currentGyroRange = 0;
jvfausto 15:b7cac36c4cce 11 currentAcceleroRange=0;
jvfausto 15:b7cac36c4cce 12 alpha = ALPHA;
jvfausto 15:b7cac36c4cce 13 }
jvfausto 15:b7cac36c4cce 14
jvfausto 15:b7cac36c4cce 15 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 16 //-------------------General------------------------
jvfausto 15:b7cac36c4cce 17 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 18
jvfausto 15:b7cac36c4cce 19 void MPU6050::write(char address, char data) {
jvfausto 15:b7cac36c4cce 20 char temp[2];
jvfausto 15:b7cac36c4cce 21 temp[0]=address;
jvfausto 15:b7cac36c4cce 22 temp[1]=data;
jvfausto 15:b7cac36c4cce 23
jvfausto 15:b7cac36c4cce 24 connection.write(MPU6050_ADDRESS * 2,temp,2);
jvfausto 15:b7cac36c4cce 25 }
jvfausto 15:b7cac36c4cce 26
jvfausto 15:b7cac36c4cce 27 char MPU6050::read(char address) {
jvfausto 15:b7cac36c4cce 28 char retval;
jvfausto 15:b7cac36c4cce 29 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
jvfausto 15:b7cac36c4cce 30 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
jvfausto 15:b7cac36c4cce 31 return retval;
jvfausto 15:b7cac36c4cce 32 }
jvfausto 15:b7cac36c4cce 33
jvfausto 15:b7cac36c4cce 34 void MPU6050::read(char address, char *data, int length) {
jvfausto 15:b7cac36c4cce 35 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
jvfausto 15:b7cac36c4cce 36 connection.read(MPU6050_ADDRESS * 2, data, length);
jvfausto 15:b7cac36c4cce 37 }
jvfausto 15:b7cac36c4cce 38
jvfausto 15:b7cac36c4cce 39 void MPU6050::setSleepMode(bool state) {
jvfausto 15:b7cac36c4cce 40 char temp;
jvfausto 15:b7cac36c4cce 41 temp = this->read(MPU6050_PWR_MGMT_1_REG);
jvfausto 15:b7cac36c4cce 42 if (state == true)
jvfausto 15:b7cac36c4cce 43 temp |= 1<<MPU6050_SLP_BIT;
jvfausto 15:b7cac36c4cce 44 if (state == false)
jvfausto 15:b7cac36c4cce 45 temp &= ~(1<<MPU6050_SLP_BIT);
jvfausto 15:b7cac36c4cce 46 this->write(MPU6050_PWR_MGMT_1_REG, temp);
jvfausto 15:b7cac36c4cce 47 }
jvfausto 15:b7cac36c4cce 48
jvfausto 15:b7cac36c4cce 49 bool MPU6050::testConnection( void ) {
jvfausto 15:b7cac36c4cce 50 char temp;
jvfausto 15:b7cac36c4cce 51 temp = this->read(MPU6050_WHO_AM_I_REG);
jvfausto 15:b7cac36c4cce 52 return (temp == (MPU6050_ADDRESS & 0xFE));
jvfausto 15:b7cac36c4cce 53 }
jvfausto 15:b7cac36c4cce 54
jvfausto 15:b7cac36c4cce 55 void MPU6050::setBW(char BW) {
jvfausto 15:b7cac36c4cce 56 char temp;
jvfausto 15:b7cac36c4cce 57 BW=BW & 0x07;
jvfausto 15:b7cac36c4cce 58 temp = this->read(MPU6050_CONFIG_REG);
jvfausto 15:b7cac36c4cce 59 temp &= 0xF8;
jvfausto 15:b7cac36c4cce 60 temp = temp + BW;
jvfausto 15:b7cac36c4cce 61 this->write(MPU6050_CONFIG_REG, temp);
jvfausto 15:b7cac36c4cce 62 }
jvfausto 15:b7cac36c4cce 63
jvfausto 15:b7cac36c4cce 64 void MPU6050::setI2CBypass(bool state) {
jvfausto 15:b7cac36c4cce 65 char temp;
jvfausto 15:b7cac36c4cce 66 temp = this->read(MPU6050_INT_PIN_CFG);
jvfausto 15:b7cac36c4cce 67 if (state == true)
jvfausto 15:b7cac36c4cce 68 temp |= 1<<MPU6050_BYPASS_BIT;
jvfausto 15:b7cac36c4cce 69 if (state == false)
jvfausto 15:b7cac36c4cce 70 temp &= ~(1<<MPU6050_BYPASS_BIT);
jvfausto 15:b7cac36c4cce 71 this->write(MPU6050_INT_PIN_CFG, temp);
jvfausto 15:b7cac36c4cce 72 }
jvfausto 15:b7cac36c4cce 73
jvfausto 15:b7cac36c4cce 74 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 75 //----------------Accelerometer---------------------
jvfausto 15:b7cac36c4cce 76 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 77
jvfausto 15:b7cac36c4cce 78 void MPU6050::setAcceleroRange( char range ) {
jvfausto 15:b7cac36c4cce 79 char temp;
jvfausto 15:b7cac36c4cce 80 range = range & 0x03;
jvfausto 15:b7cac36c4cce 81 currentAcceleroRange = range;
jvfausto 15:b7cac36c4cce 82
jvfausto 15:b7cac36c4cce 83 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
jvfausto 15:b7cac36c4cce 84 temp &= ~(3<<3);
jvfausto 15:b7cac36c4cce 85 temp = temp + (range<<3);
jvfausto 15:b7cac36c4cce 86 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
jvfausto 15:b7cac36c4cce 87 }
jvfausto 15:b7cac36c4cce 88
jvfausto 15:b7cac36c4cce 89 int MPU6050::getAcceleroRawX( void ) {
jvfausto 15:b7cac36c4cce 90 short retval;
jvfausto 15:b7cac36c4cce 91 char data[2];
jvfausto 15:b7cac36c4cce 92 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 93 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 94 return (int)retval;
jvfausto 15:b7cac36c4cce 95 }
jvfausto 15:b7cac36c4cce 96
jvfausto 15:b7cac36c4cce 97 int MPU6050::getAcceleroRawY( void ) {
jvfausto 15:b7cac36c4cce 98 short retval;
jvfausto 15:b7cac36c4cce 99 char data[2];
jvfausto 15:b7cac36c4cce 100 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 101 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 102 return (int)retval;
jvfausto 15:b7cac36c4cce 103 }
jvfausto 15:b7cac36c4cce 104
jvfausto 15:b7cac36c4cce 105 int MPU6050::getAcceleroRawZ( void ) {
jvfausto 15:b7cac36c4cce 106 short retval;
jvfausto 15:b7cac36c4cce 107 char data[2];
jvfausto 15:b7cac36c4cce 108 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 109 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 110 return (int)retval;
jvfausto 15:b7cac36c4cce 111 }
jvfausto 15:b7cac36c4cce 112
jvfausto 15:b7cac36c4cce 113 void MPU6050::getAcceleroRaw( int *data ) {
jvfausto 15:b7cac36c4cce 114 char temp[6];
jvfausto 15:b7cac36c4cce 115 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
jvfausto 15:b7cac36c4cce 116 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
jvfausto 15:b7cac36c4cce 117 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
jvfausto 15:b7cac36c4cce 118 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
jvfausto 15:b7cac36c4cce 119 }
jvfausto 15:b7cac36c4cce 120
jvfausto 15:b7cac36c4cce 121 void MPU6050::getAccelero( float *data ) {
jvfausto 15:b7cac36c4cce 122 int temp[3];
jvfausto 15:b7cac36c4cce 123 this->getAcceleroRaw(temp);
jvfausto 15:b7cac36c4cce 124 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
jvfausto 15:b7cac36c4cce 125 data[0]=(float)temp[0] / 16384.0 * 9.81;
jvfausto 15:b7cac36c4cce 126 data[1]=(float)temp[1] / 16384.0 * 9.81;
jvfausto 15:b7cac36c4cce 127 data[2]=(float)temp[2] / 16384.0 * 9.81;
jvfausto 15:b7cac36c4cce 128 }
jvfausto 15:b7cac36c4cce 129 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
jvfausto 15:b7cac36c4cce 130 data[0]=(float)temp[0] / 8192.0 * 9.81;
jvfausto 15:b7cac36c4cce 131 data[1]=(float)temp[1] / 8192.0 * 9.81;
jvfausto 15:b7cac36c4cce 132 data[2]=(float)temp[2] / 8192.0 * 9.81;
jvfausto 15:b7cac36c4cce 133 }
jvfausto 15:b7cac36c4cce 134 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
jvfausto 15:b7cac36c4cce 135 data[0]=(float)temp[0] / 4096.0 * 9.81;
jvfausto 15:b7cac36c4cce 136 data[1]=(float)temp[1] / 4096.0 * 9.81;
jvfausto 15:b7cac36c4cce 137 data[2]=(float)temp[2] / 4096.0 * 9.81;
jvfausto 15:b7cac36c4cce 138 }
jvfausto 15:b7cac36c4cce 139 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
jvfausto 15:b7cac36c4cce 140 data[0]=(float)temp[0] / 2048.0 * 9.81;
jvfausto 15:b7cac36c4cce 141 data[1]=(float)temp[1] / 2048.0 * 9.81;
jvfausto 15:b7cac36c4cce 142 data[2]=(float)temp[2] / 2048.0 * 9.81;
jvfausto 15:b7cac36c4cce 143 }
jvfausto 15:b7cac36c4cce 144
jvfausto 15:b7cac36c4cce 145 #ifdef DOUBLE_ACCELERO
jvfausto 15:b7cac36c4cce 146 data[0]*=2;
jvfausto 15:b7cac36c4cce 147 data[1]*=2;
jvfausto 15:b7cac36c4cce 148 data[2]*=2;
jvfausto 15:b7cac36c4cce 149 #endif
jvfausto 15:b7cac36c4cce 150 }
jvfausto 15:b7cac36c4cce 151 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 152 //------------------Gyroscope-----------------------
jvfausto 15:b7cac36c4cce 153 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 154 void MPU6050::setGyroRange( char range ) {
jvfausto 15:b7cac36c4cce 155 char temp;
jvfausto 15:b7cac36c4cce 156 currentGyroRange = range;
jvfausto 15:b7cac36c4cce 157 range = range & 0x03;
jvfausto 15:b7cac36c4cce 158 temp = this->read(MPU6050_GYRO_CONFIG_REG);
jvfausto 15:b7cac36c4cce 159 temp &= ~(3<<3);
jvfausto 15:b7cac36c4cce 160 temp = temp + range<<3;
jvfausto 15:b7cac36c4cce 161 this->write(MPU6050_GYRO_CONFIG_REG, temp);
jvfausto 15:b7cac36c4cce 162 }
jvfausto 15:b7cac36c4cce 163
jvfausto 15:b7cac36c4cce 164 int MPU6050::getGyroRawX( void ) {
jvfausto 15:b7cac36c4cce 165 short retval;
jvfausto 15:b7cac36c4cce 166 char data[2];
jvfausto 15:b7cac36c4cce 167 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 168 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 169 return (int)retval;
jvfausto 15:b7cac36c4cce 170 }
jvfausto 15:b7cac36c4cce 171
jvfausto 15:b7cac36c4cce 172 int MPU6050::getGyroRawY( void ) {
jvfausto 15:b7cac36c4cce 173 short retval;
jvfausto 15:b7cac36c4cce 174 char data[2];
jvfausto 15:b7cac36c4cce 175 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 176 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 177 return (int)retval;
jvfausto 15:b7cac36c4cce 178 }
jvfausto 15:b7cac36c4cce 179
jvfausto 15:b7cac36c4cce 180 int MPU6050::getGyroRawZ( void ) {
jvfausto 15:b7cac36c4cce 181 short retval;
jvfausto 15:b7cac36c4cce 182 char data[2];
jvfausto 15:b7cac36c4cce 183 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 184 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 185 return (int)retval;
jvfausto 15:b7cac36c4cce 186 }
jvfausto 15:b7cac36c4cce 187
jvfausto 15:b7cac36c4cce 188 void MPU6050::getGyroRaw( int *data ) {
jvfausto 15:b7cac36c4cce 189 char temp[6];
jvfausto 15:b7cac36c4cce 190 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
jvfausto 15:b7cac36c4cce 191 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
jvfausto 15:b7cac36c4cce 192 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
jvfausto 15:b7cac36c4cce 193 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
jvfausto 15:b7cac36c4cce 194 }
jvfausto 15:b7cac36c4cce 195
jvfausto 15:b7cac36c4cce 196 void MPU6050::getGyro( float *data ) {
jvfausto 15:b7cac36c4cce 197 int temp[3];
jvfausto 15:b7cac36c4cce 198 this->getGyroRaw(temp);
jvfausto 15:b7cac36c4cce 199 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
jvfausto 15:b7cac36c4cce 200 data[0]=(float)temp[0] / 301.0;
jvfausto 15:b7cac36c4cce 201 data[1]=(float)temp[1] / 301.0;
jvfausto 15:b7cac36c4cce 202 data[2]=(float)temp[2] / 301.0;
jvfausto 15:b7cac36c4cce 203 } //7505.5
jvfausto 15:b7cac36c4cce 204 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
jvfausto 15:b7cac36c4cce 205 data[0]=(float)temp[0] / 3752.9;
jvfausto 15:b7cac36c4cce 206 data[1]=(float)temp[1] / 3752.9;
jvfausto 15:b7cac36c4cce 207 data[2]=(float)temp[2] / 3752.9;
jvfausto 15:b7cac36c4cce 208 }
jvfausto 15:b7cac36c4cce 209 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
jvfausto 15:b7cac36c4cce 210 data[0]=(float)temp[0]/ 1879.3;
jvfausto 15:b7cac36c4cce 211 data[1]=(float)temp[1] / 1879.3;
jvfausto 15:b7cac36c4cce 212 data[2]=(float)temp[2] / 1879.3;
jvfausto 15:b7cac36c4cce 213
jvfausto 15:b7cac36c4cce 214 }
jvfausto 15:b7cac36c4cce 215 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
jvfausto 15:b7cac36c4cce 216 data[0]=(float)temp[0] / 939.7;
jvfausto 15:b7cac36c4cce 217 data[1]=(float)temp[1] / 939.7;
jvfausto 15:b7cac36c4cce 218 data[2]=(float)temp[2] / 939.7;
jvfausto 15:b7cac36c4cce 219 }
jvfausto 15:b7cac36c4cce 220
jvfausto 15:b7cac36c4cce 221 }
jvfausto 15:b7cac36c4cce 222 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 223 //-------------------Temperature--------------------
jvfausto 15:b7cac36c4cce 224 //--------------------------------------------------
jvfausto 15:b7cac36c4cce 225 int MPU6050::getTempRaw( void ) {
jvfausto 15:b7cac36c4cce 226 short retval;
jvfausto 15:b7cac36c4cce 227 char data[2];
jvfausto 15:b7cac36c4cce 228 this->read(MPU6050_TEMP_H_REG, data, 2);
jvfausto 15:b7cac36c4cce 229 retval = (data[0]<<8) + data[1];
jvfausto 15:b7cac36c4cce 230 return (int)retval;
jvfausto 15:b7cac36c4cce 231 }
jvfausto 15:b7cac36c4cce 232
jvfausto 15:b7cac36c4cce 233 float MPU6050::getTemp( void ) {
jvfausto 15:b7cac36c4cce 234 float retval;
jvfausto 15:b7cac36c4cce 235 retval=(float)this->getTempRaw();
jvfausto 15:b7cac36c4cce 236 retval=(retval+521.0)/340.0+35.0;
jvfausto 15:b7cac36c4cce 237 return retval;
jvfausto 15:b7cac36c4cce 238 }
jvfausto 15:b7cac36c4cce 239
jvfausto 15:b7cac36c4cce 240 /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more.
jvfausto 15:b7cac36c4cce 241 function for getting angles in degrees from accelerometer
jvfausto 15:b7cac36c4cce 242 */
jvfausto 15:b7cac36c4cce 243 void MPU6050::getAcceleroAngle( float *data ) {
jvfausto 15:b7cac36c4cce 244 float temp[3];
jvfausto 15:b7cac36c4cce 245 this->getAccelero(temp);
jvfausto 15:b7cac36c4cce 246
jvfausto 15:b7cac36c4cce 247 data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
jvfausto 15:b7cac36c4cce 248 data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
jvfausto 15:b7cac36c4cce 249 data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on
jvfausto 15:b7cac36c4cce 250
jvfausto 15:b7cac36c4cce 251 // data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees)
jvfausto 15:b7cac36c4cce 252 // data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES; //but it takes longer and system gets unstable when angles ~90 degrees
jvfausto 15:b7cac36c4cce 253 }
jvfausto 15:b7cac36c4cce 254
jvfausto 15:b7cac36c4cce 255 ///function for getting offset values for the gyro & accelerometer
jvfausto 15:b7cac36c4cce 256 void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
jvfausto 15:b7cac36c4cce 257 float gyro[3];
jvfausto 15:b7cac36c4cce 258 float accAngle[3];
jvfausto 15:b7cac36c4cce 259
jvfausto 15:b7cac36c4cce 260 for (int i = 0; i < 3; i++) {
jvfausto 15:b7cac36c4cce 261 accOffset[i] = 0.0; //initialise offsets to 0.0
jvfausto 15:b7cac36c4cce 262 gyroOffset[i] = 0.0;
jvfausto 15:b7cac36c4cce 263 }
jvfausto 15:b7cac36c4cce 264
jvfausto 15:b7cac36c4cce 265 for (int i = 0; i < sampleSize; i++){
jvfausto 15:b7cac36c4cce 266 this->getGyro(gyro); //take real life measurements
jvfausto 15:b7cac36c4cce 267 this->getAcceleroAngle (accAngle);
jvfausto 15:b7cac36c4cce 268
jvfausto 15:b7cac36c4cce 269 for (int j = 0; j < 3; j++){
jvfausto 15:b7cac36c4cce 270 *(accOffset+j) += accAngle[j]/sampleSize; //average measurements
jvfausto 15:b7cac36c4cce 271 *(gyroOffset+j) += gyro[j]/sampleSize;
jvfausto 15:b7cac36c4cce 272 }
jvfausto 15:b7cac36c4cce 273 wait (0.01); //wait between each reading for accuracy
jvfausto 15:b7cac36c4cce 274 }
jvfausto 15:b7cac36c4cce 275 }
jvfausto 15:b7cac36c4cce 276
jvfausto 15:b7cac36c4cce 277 ///function for computing angles for roll, pitch anf yaw
jvfausto 15:b7cac36c4cce 278 void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){
jvfausto 15:b7cac36c4cce 279 float gyro[3];
jvfausto 15:b7cac36c4cce 280 float accAngle[3];
jvfausto 15:b7cac36c4cce 281
jvfausto 15:b7cac36c4cce 282 this->getGyro(gyro); //get gyro value in rad/s
jvfausto 15:b7cac36c4cce 283 this->getAcceleroAngle(accAngle); //get angle from accelerometer
jvfausto 15:b7cac36c4cce 284
jvfausto 15:b7cac36c4cce 285 for (int i = 0; i < 3; i++){
jvfausto 15:b7cac36c4cce 286 gyro[i] -= gyroOffset[i]; //substract offset values
jvfausto 15:b7cac36c4cce 287 accAngle[i] -= accOffset[i];
jvfausto 15:b7cac36c4cce 288 }
jvfausto 15:b7cac36c4cce 289
jvfausto 15:b7cac36c4cce 290 //apply filters on pitch and roll to get accurate angle values
jvfausto 15:b7cac36c4cce 291 angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS];
jvfausto 15:b7cac36c4cce 292 angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS];
jvfausto 15:b7cac36c4cce 293
jvfausto 15:b7cac36c4cce 294 //calculate Yaw using just the gyroscope values - inaccurate
jvfausto 15:b7cac36c4cce 295 angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval;
jvfausto 15:b7cac36c4cce 296 }
jvfausto 15:b7cac36c4cce 297
jvfausto 15:b7cac36c4cce 298 ///function for setting a different Alpha value, which is used in complemetary filter calculations
jvfausto 15:b7cac36c4cce 299 void MPU6050::setAlpha(float val){
jvfausto 15:b7cac36c4cce 300 alpha = val;
jvfausto 15:b7cac36c4cce 301 }
jvfausto 15:b7cac36c4cce 302
jvfausto 15:b7cac36c4cce 303 ///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take
jvfausto 15:b7cac36c4cce 304 void MPU6050::enableInt( void ){
jvfausto 15:b7cac36c4cce 305 char temp;
jvfausto 15:b7cac36c4cce 306 temp = this->read(MPU6050_RA_INT_ENABLE);
jvfausto 15:b7cac36c4cce 307 temp |= 0x01;
jvfausto 15:b7cac36c4cce 308 this->write(MPU6050_RA_INT_ENABLE, temp);
jvfausto 15:b7cac36c4cce 309 }
jvfausto 15:b7cac36c4cce 310
jvfausto 15:b7cac36c4cce 311 ///function for disabling interrupts on MPU6050 INT pin, when the data is ready to take
jvfausto 15:b7cac36c4cce 312 void MPU6050::disableInt ( void ){
jvfausto 15:b7cac36c4cce 313 char temp;
jvfausto 15:b7cac36c4cce 314 temp = this->read(MPU6050_RA_INT_ENABLE);
jvfausto 15:b7cac36c4cce 315 temp &= 0xFE;
jvfausto 15:b7cac36c4cce 316 this->write(MPU6050_RA_INT_ENABLE, temp);
jvfausto 15:b7cac36c4cce 317 }