Thanks Erik Olieman for his beta library, that saved me a huge amount of time when getting Raw data from MPU6050 module! I was able to update this library by adding additional functions, which would allow a fast angle calculation by using complementary filter. I will probably be updating this library more soon to add additional functionality or make some changes that would look sensible.
Dependents: QuadcopterProgram 3DTracking ControlYokutan2017 Gyro ... more
Fork of MPU6050 by
MPU6050.cpp@8:b1570b99df9e, 2015-02-13 (annotated)
- Committer:
- moklumbys
- Date:
- Fri Feb 13 01:16:00 2015 +0000
- Revision:
- 8:b1570b99df9e
- Parent:
- 7:56e591a74939
- Child:
- 9:898effccce30
Added one more function, which allows enabling interrupts on Int pin. Now we can gather data only when it is received. Next maybe possible to define time period for getting data...
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sissors | 0:6757f7363a9f | 1 | /** |
Sissors | 0:6757f7363a9f | 2 | * Includes |
Sissors | 0:6757f7363a9f | 3 | */ |
Sissors | 0:6757f7363a9f | 4 | #include "MPU6050.h" |
Sissors | 0:6757f7363a9f | 5 | |
Sissors | 0:6757f7363a9f | 6 | MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { |
Sissors | 0:6757f7363a9f | 7 | this->setSleepMode(false); |
Sissors | 0:6757f7363a9f | 8 | |
Sissors | 0:6757f7363a9f | 9 | //Initializations: |
Sissors | 0:6757f7363a9f | 10 | currentGyroRange = 0; |
Sissors | 0:6757f7363a9f | 11 | currentAcceleroRange=0; |
Sissors | 0:6757f7363a9f | 12 | } |
Sissors | 0:6757f7363a9f | 13 | |
Sissors | 0:6757f7363a9f | 14 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 15 | //-------------------General------------------------ |
Sissors | 0:6757f7363a9f | 16 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 17 | |
Sissors | 0:6757f7363a9f | 18 | void MPU6050::write(char address, char data) { |
Sissors | 0:6757f7363a9f | 19 | char temp[2]; |
Sissors | 0:6757f7363a9f | 20 | temp[0]=address; |
Sissors | 0:6757f7363a9f | 21 | temp[1]=data; |
Sissors | 0:6757f7363a9f | 22 | |
Sissors | 0:6757f7363a9f | 23 | connection.write(MPU6050_ADDRESS * 2,temp,2); |
Sissors | 0:6757f7363a9f | 24 | } |
Sissors | 0:6757f7363a9f | 25 | |
Sissors | 0:6757f7363a9f | 26 | char MPU6050::read(char address) { |
Sissors | 0:6757f7363a9f | 27 | char retval; |
Sissors | 0:6757f7363a9f | 28 | connection.write(MPU6050_ADDRESS * 2, &address, 1, true); |
Sissors | 0:6757f7363a9f | 29 | connection.read(MPU6050_ADDRESS * 2, &retval, 1); |
Sissors | 0:6757f7363a9f | 30 | return retval; |
Sissors | 0:6757f7363a9f | 31 | } |
Sissors | 0:6757f7363a9f | 32 | |
Sissors | 0:6757f7363a9f | 33 | void MPU6050::read(char address, char *data, int length) { |
Sissors | 0:6757f7363a9f | 34 | connection.write(MPU6050_ADDRESS * 2, &address, 1, true); |
Sissors | 0:6757f7363a9f | 35 | connection.read(MPU6050_ADDRESS * 2, data, length); |
Sissors | 0:6757f7363a9f | 36 | } |
Sissors | 0:6757f7363a9f | 37 | |
Sissors | 0:6757f7363a9f | 38 | void MPU6050::setSleepMode(bool state) { |
Sissors | 0:6757f7363a9f | 39 | char temp; |
Sissors | 0:6757f7363a9f | 40 | temp = this->read(MPU6050_PWR_MGMT_1_REG); |
Sissors | 0:6757f7363a9f | 41 | if (state == true) |
Sissors | 0:6757f7363a9f | 42 | temp |= 1<<MPU6050_SLP_BIT; |
Sissors | 0:6757f7363a9f | 43 | if (state == false) |
Sissors | 0:6757f7363a9f | 44 | temp &= ~(1<<MPU6050_SLP_BIT); |
Sissors | 0:6757f7363a9f | 45 | this->write(MPU6050_PWR_MGMT_1_REG, temp); |
Sissors | 0:6757f7363a9f | 46 | } |
Sissors | 0:6757f7363a9f | 47 | |
Sissors | 0:6757f7363a9f | 48 | bool MPU6050::testConnection( void ) { |
Sissors | 0:6757f7363a9f | 49 | char temp; |
Sissors | 0:6757f7363a9f | 50 | temp = this->read(MPU6050_WHO_AM_I_REG); |
Sissors | 0:6757f7363a9f | 51 | return (temp == (MPU6050_ADDRESS & 0xFE)); |
Sissors | 0:6757f7363a9f | 52 | } |
Sissors | 0:6757f7363a9f | 53 | |
Sissors | 0:6757f7363a9f | 54 | void MPU6050::setBW(char BW) { |
Sissors | 0:6757f7363a9f | 55 | char temp; |
Sissors | 0:6757f7363a9f | 56 | BW=BW & 0x07; |
Sissors | 0:6757f7363a9f | 57 | temp = this->read(MPU6050_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 58 | temp &= 0xF8; |
Sissors | 0:6757f7363a9f | 59 | temp = temp + BW; |
Sissors | 0:6757f7363a9f | 60 | this->write(MPU6050_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 61 | } |
Sissors | 0:6757f7363a9f | 62 | |
Sissors | 0:6757f7363a9f | 63 | void MPU6050::setI2CBypass(bool state) { |
Sissors | 0:6757f7363a9f | 64 | char temp; |
Sissors | 0:6757f7363a9f | 65 | temp = this->read(MPU6050_INT_PIN_CFG); |
Sissors | 0:6757f7363a9f | 66 | if (state == true) |
Sissors | 0:6757f7363a9f | 67 | temp |= 1<<MPU6050_BYPASS_BIT; |
Sissors | 0:6757f7363a9f | 68 | if (state == false) |
Sissors | 0:6757f7363a9f | 69 | temp &= ~(1<<MPU6050_BYPASS_BIT); |
Sissors | 0:6757f7363a9f | 70 | this->write(MPU6050_INT_PIN_CFG, temp); |
Sissors | 0:6757f7363a9f | 71 | } |
Sissors | 0:6757f7363a9f | 72 | |
Sissors | 0:6757f7363a9f | 73 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 74 | //----------------Accelerometer--------------------- |
Sissors | 0:6757f7363a9f | 75 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 76 | |
Sissors | 0:6757f7363a9f | 77 | void MPU6050::setAcceleroRange( char range ) { |
Sissors | 0:6757f7363a9f | 78 | char temp; |
Sissors | 0:6757f7363a9f | 79 | range = range & 0x03; |
Sissors | 0:6757f7363a9f | 80 | currentAcceleroRange = range; |
Sissors | 0:6757f7363a9f | 81 | |
Sissors | 0:6757f7363a9f | 82 | temp = this->read(MPU6050_ACCELERO_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 83 | temp &= ~(3<<3); |
Sissors | 0:6757f7363a9f | 84 | temp = temp + (range<<3); |
Sissors | 0:6757f7363a9f | 85 | this->write(MPU6050_ACCELERO_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 86 | } |
Sissors | 0:6757f7363a9f | 87 | |
Sissors | 0:6757f7363a9f | 88 | int MPU6050::getAcceleroRawX( void ) { |
Sissors | 0:6757f7363a9f | 89 | short retval; |
Sissors | 0:6757f7363a9f | 90 | char data[2]; |
Sissors | 0:6757f7363a9f | 91 | this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 92 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 93 | return (int)retval; |
Sissors | 0:6757f7363a9f | 94 | } |
Sissors | 0:6757f7363a9f | 95 | |
Sissors | 0:6757f7363a9f | 96 | int MPU6050::getAcceleroRawY( void ) { |
Sissors | 0:6757f7363a9f | 97 | short retval; |
Sissors | 0:6757f7363a9f | 98 | char data[2]; |
Sissors | 0:6757f7363a9f | 99 | this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 100 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 101 | return (int)retval; |
Sissors | 0:6757f7363a9f | 102 | } |
Sissors | 0:6757f7363a9f | 103 | |
Sissors | 0:6757f7363a9f | 104 | int MPU6050::getAcceleroRawZ( void ) { |
Sissors | 0:6757f7363a9f | 105 | short retval; |
Sissors | 0:6757f7363a9f | 106 | char data[2]; |
Sissors | 0:6757f7363a9f | 107 | this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 108 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 109 | return (int)retval; |
Sissors | 0:6757f7363a9f | 110 | } |
Sissors | 0:6757f7363a9f | 111 | |
Sissors | 0:6757f7363a9f | 112 | void MPU6050::getAcceleroRaw( int *data ) { |
Sissors | 0:6757f7363a9f | 113 | char temp[6]; |
Sissors | 0:6757f7363a9f | 114 | this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); |
Sissors | 0:6757f7363a9f | 115 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
Sissors | 0:6757f7363a9f | 116 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
Sissors | 0:6757f7363a9f | 117 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
Sissors | 0:6757f7363a9f | 118 | } |
Sissors | 0:6757f7363a9f | 119 | |
Sissors | 0:6757f7363a9f | 120 | void MPU6050::getAccelero( float *data ) { |
Sissors | 0:6757f7363a9f | 121 | int temp[3]; |
Sissors | 0:6757f7363a9f | 122 | this->getAcceleroRaw(temp); |
Sissors | 0:6757f7363a9f | 123 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { |
Sissors | 0:6757f7363a9f | 124 | data[0]=(float)temp[0] / 16384.0 * 9.81; |
Sissors | 0:6757f7363a9f | 125 | data[1]=(float)temp[1] / 16384.0 * 9.81; |
Sissors | 0:6757f7363a9f | 126 | data[2]=(float)temp[2] / 16384.0 * 9.81; |
Sissors | 0:6757f7363a9f | 127 | } |
Sissors | 0:6757f7363a9f | 128 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ |
Sissors | 0:6757f7363a9f | 129 | data[0]=(float)temp[0] / 8192.0 * 9.81; |
Sissors | 0:6757f7363a9f | 130 | data[1]=(float)temp[1] / 8192.0 * 9.81; |
Sissors | 0:6757f7363a9f | 131 | data[2]=(float)temp[2] / 8192.0 * 9.81; |
Sissors | 0:6757f7363a9f | 132 | } |
Sissors | 0:6757f7363a9f | 133 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ |
Sissors | 0:6757f7363a9f | 134 | data[0]=(float)temp[0] / 4096.0 * 9.81; |
Sissors | 0:6757f7363a9f | 135 | data[1]=(float)temp[1] / 4096.0 * 9.81; |
Sissors | 0:6757f7363a9f | 136 | data[2]=(float)temp[2] / 4096.0 * 9.81; |
Sissors | 0:6757f7363a9f | 137 | } |
Sissors | 0:6757f7363a9f | 138 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ |
Sissors | 0:6757f7363a9f | 139 | data[0]=(float)temp[0] / 2048.0 * 9.81; |
Sissors | 0:6757f7363a9f | 140 | data[1]=(float)temp[1] / 2048.0 * 9.81; |
Sissors | 0:6757f7363a9f | 141 | data[2]=(float)temp[2] / 2048.0 * 9.81; |
Sissors | 0:6757f7363a9f | 142 | } |
Sissors | 0:6757f7363a9f | 143 | |
Sissors | 0:6757f7363a9f | 144 | #ifdef DOUBLE_ACCELERO |
Sissors | 0:6757f7363a9f | 145 | data[0]*=2; |
Sissors | 0:6757f7363a9f | 146 | data[1]*=2; |
Sissors | 0:6757f7363a9f | 147 | data[2]*=2; |
Sissors | 0:6757f7363a9f | 148 | #endif |
Sissors | 0:6757f7363a9f | 149 | } |
Sissors | 0:6757f7363a9f | 150 | |
moklumbys | 4:268d3fcb92ba | 151 | //function for getting angles from accelerometer |
moklumbys | 4:268d3fcb92ba | 152 | void MPU6050::getAcceleroAngle( float *data ) { |
moklumbys | 6:502448484f91 | 153 | float temp[3]; |
moklumbys | 4:268d3fcb92ba | 154 | this->getAccelero(temp); |
moklumbys | 4:268d3fcb92ba | 155 | |
moklumbys | 4:268d3fcb92ba | 156 | data[X_AXIS] = -1*atan (-1*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 |
moklumbys | 4:268d3fcb92ba | 157 | data[Y_AXIS] = -1*atan (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 |
moklumbys | 4:268d3fcb92ba | 158 | data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //not sure what is this one :D |
moklumbys | 4:268d3fcb92ba | 159 | |
moklumbys | 4:268d3fcb92ba | 160 | } |
moklumbys | 5:5873df1e58be | 161 | void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ |
moklumbys | 5:5873df1e58be | 162 | float gyro[3]; |
moklumbys | 5:5873df1e58be | 163 | float accAngle[3]; |
moklumbys | 5:5873df1e58be | 164 | |
moklumbys | 5:5873df1e58be | 165 | for (int i = 0; i < 3; i++) //initialise to 0.0 offsets |
moklumbys | 5:5873df1e58be | 166 | { |
moklumbys | 5:5873df1e58be | 167 | accOffset[i] = 0.0; |
moklumbys | 5:5873df1e58be | 168 | gyroOffset[i] = 0.0; |
moklumbys | 5:5873df1e58be | 169 | } |
moklumbys | 5:5873df1e58be | 170 | |
moklumbys | 5:5873df1e58be | 171 | for (int i = 0; i < sampleSize; i++) |
moklumbys | 5:5873df1e58be | 172 | { |
moklumbys | 6:502448484f91 | 173 | this->getGyro(gyro); //take real life measurements |
moklumbys | 6:502448484f91 | 174 | this->getAcceleroAngle (accAngle); |
moklumbys | 5:5873df1e58be | 175 | |
moklumbys | 5:5873df1e58be | 176 | for (int j = 0; j < 3; j++) |
moklumbys | 5:5873df1e58be | 177 | { |
moklumbys | 5:5873df1e58be | 178 | *(accOffset+j) += accAngle[j]/sampleSize; //average measurements |
moklumbys | 5:5873df1e58be | 179 | *(gyroOffset+j) += gyro[j]/sampleSize; |
moklumbys | 5:5873df1e58be | 180 | } |
moklumbys | 5:5873df1e58be | 181 | wait (0.01); //wait between each reading for accuracy (maybe will not need later on) ****))#$(#@)$@#)(#@(%)@#(%@)(#%#@$-------------- |
moklumbys | 5:5873df1e58be | 182 | } |
moklumbys | 5:5873df1e58be | 183 | } |
moklumbys | 7:56e591a74939 | 184 | |
moklumbys | 7:56e591a74939 | 185 | void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime) |
moklumbys | 7:56e591a74939 | 186 | { |
moklumbys | 7:56e591a74939 | 187 | float gyro[3]; |
moklumbys | 7:56e591a74939 | 188 | float accAngle[3]; |
moklumbys | 7:56e591a74939 | 189 | |
moklumbys | 7:56e591a74939 | 190 | this->getGyro(gyro); |
moklumbys | 7:56e591a74939 | 191 | this->getAcceleroAngle(accAngle); |
moklumbys | 7:56e591a74939 | 192 | |
moklumbys | 7:56e591a74939 | 193 | for (int i = 0; i < 3; i++) |
moklumbys | 7:56e591a74939 | 194 | { |
moklumbys | 7:56e591a74939 | 195 | gyro[i] -= gyroOffset[i]; |
moklumbys | 7:56e591a74939 | 196 | accAngle[i] -= accOffset[i]; |
moklumbys | 7:56e591a74939 | 197 | } |
moklumbys | 7:56e591a74939 | 198 | |
moklumbys | 7:56e591a74939 | 199 | angle[X_AXIS] = ALPHA * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[X_AXIS]; //apply filter on both reading to get all angles |
moklumbys | 7:56e591a74939 | 200 | angle[Y_AXIS] = ALPHA * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Y_AXIS]; |
moklumbys | 7:56e591a74939 | 201 | //angle[Z_AXIS] = ALPHA * (angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Z_AXIS]; |
moklumbys | 7:56e591a74939 | 202 | angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime); //this is Yaw hopefully :D |
moklumbys | 7:56e591a74939 | 203 | //Y = atan2(rawY,rawZ) * 180 / PI; //This spits out values between -180 to 180 (360 degrees) |
moklumbys | 7:56e591a74939 | 204 | } |
moklumbys | 8:b1570b99df9e | 205 | |
moklumbys | 8:b1570b99df9e | 206 | void MPU6050::enableInt( void ) |
moklumbys | 8:b1570b99df9e | 207 | { |
moklumbys | 8:b1570b99df9e | 208 | char temp; |
moklumbys | 8:b1570b99df9e | 209 | temp = this->read(MPU6050_RA_INT_ENABLE); |
moklumbys | 8:b1570b99df9e | 210 | temp |= 0x01; |
moklumbys | 8:b1570b99df9e | 211 | this->write(MPU6050_RA_INT_ENABLE, temp); |
moklumbys | 8:b1570b99df9e | 212 | } |
moklumbys | 8:b1570b99df9e | 213 | void MPU6050::disableInt ( void ) |
moklumbys | 8:b1570b99df9e | 214 | { |
moklumbys | 8:b1570b99df9e | 215 | char temp; |
moklumbys | 8:b1570b99df9e | 216 | temp = this->read(MPU6050_RA_INT_ENABLE); |
moklumbys | 8:b1570b99df9e | 217 | temp &= 0xFE; |
moklumbys | 8:b1570b99df9e | 218 | this->write(MPU6050_RA_INT_ENABLE, temp); |
moklumbys | 8:b1570b99df9e | 219 | } |
Sissors | 0:6757f7363a9f | 220 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 221 | //------------------Gyroscope----------------------- |
Sissors | 0:6757f7363a9f | 222 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 223 | void MPU6050::setGyroRange( char range ) { |
Sissors | 0:6757f7363a9f | 224 | char temp; |
Sissors | 0:6757f7363a9f | 225 | currentGyroRange = range; |
Sissors | 0:6757f7363a9f | 226 | range = range & 0x03; |
Sissors | 0:6757f7363a9f | 227 | temp = this->read(MPU6050_GYRO_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 228 | temp &= ~(3<<3); |
Sissors | 0:6757f7363a9f | 229 | temp = temp + range<<3; |
Sissors | 0:6757f7363a9f | 230 | this->write(MPU6050_GYRO_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 231 | } |
Sissors | 0:6757f7363a9f | 232 | |
Sissors | 0:6757f7363a9f | 233 | int MPU6050::getGyroRawX( void ) { |
Sissors | 0:6757f7363a9f | 234 | short retval; |
Sissors | 0:6757f7363a9f | 235 | char data[2]; |
Sissors | 0:6757f7363a9f | 236 | this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 237 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 238 | return (int)retval; |
Sissors | 0:6757f7363a9f | 239 | } |
Sissors | 0:6757f7363a9f | 240 | |
Sissors | 0:6757f7363a9f | 241 | int MPU6050::getGyroRawY( void ) { |
Sissors | 0:6757f7363a9f | 242 | short retval; |
Sissors | 0:6757f7363a9f | 243 | char data[2]; |
Sissors | 0:6757f7363a9f | 244 | this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 245 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 246 | return (int)retval; |
Sissors | 0:6757f7363a9f | 247 | } |
Sissors | 0:6757f7363a9f | 248 | |
Sissors | 0:6757f7363a9f | 249 | int MPU6050::getGyroRawZ( void ) { |
Sissors | 0:6757f7363a9f | 250 | short retval; |
Sissors | 0:6757f7363a9f | 251 | char data[2]; |
Sissors | 0:6757f7363a9f | 252 | this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 253 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 254 | return (int)retval; |
Sissors | 0:6757f7363a9f | 255 | } |
Sissors | 0:6757f7363a9f | 256 | |
Sissors | 0:6757f7363a9f | 257 | void MPU6050::getGyroRaw( int *data ) { |
Sissors | 0:6757f7363a9f | 258 | char temp[6]; |
Sissors | 0:6757f7363a9f | 259 | this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); |
Sissors | 0:6757f7363a9f | 260 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
Sissors | 0:6757f7363a9f | 261 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
Sissors | 0:6757f7363a9f | 262 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
Sissors | 0:6757f7363a9f | 263 | } |
Sissors | 0:6757f7363a9f | 264 | |
Sissors | 0:6757f7363a9f | 265 | void MPU6050::getGyro( float *data ) { |
Sissors | 0:6757f7363a9f | 266 | int temp[3]; |
Sissors | 0:6757f7363a9f | 267 | this->getGyroRaw(temp); |
Sissors | 1:a3366f09e95c | 268 | if (currentGyroRange == MPU6050_GYRO_RANGE_250) { |
moklumbys | 3:187152513f8d | 269 | data[0]=(float)temp[0] / 301.0; |
moklumbys | 3:187152513f8d | 270 | data[1]=(float)temp[1] / 301.0; |
moklumbys | 3:187152513f8d | 271 | data[2]=(float)temp[2] / 301.0; |
moklumbys | 3:187152513f8d | 272 | } //7505.5 |
Sissors | 1:a3366f09e95c | 273 | if (currentGyroRange == MPU6050_GYRO_RANGE_500){ |
Sissors | 0:6757f7363a9f | 274 | data[0]=(float)temp[0] / 3752.9; |
Sissors | 0:6757f7363a9f | 275 | data[1]=(float)temp[1] / 3752.9; |
Sissors | 0:6757f7363a9f | 276 | data[2]=(float)temp[2] / 3752.9; |
Sissors | 0:6757f7363a9f | 277 | } |
Sissors | 1:a3366f09e95c | 278 | if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ |
Sissors | 0:6757f7363a9f | 279 | data[0]=(float)temp[0] / 1879.3;; |
Sissors | 0:6757f7363a9f | 280 | data[1]=(float)temp[1] / 1879.3; |
Sissors | 0:6757f7363a9f | 281 | data[2]=(float)temp[2] / 1879.3; |
Sissors | 0:6757f7363a9f | 282 | } |
Sissors | 1:a3366f09e95c | 283 | if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ |
Sissors | 0:6757f7363a9f | 284 | data[0]=(float)temp[0] / 939.7; |
Sissors | 0:6757f7363a9f | 285 | data[1]=(float)temp[1] / 939.7; |
Sissors | 0:6757f7363a9f | 286 | data[2]=(float)temp[2] / 939.7; |
Sissors | 0:6757f7363a9f | 287 | } |
Sissors | 0:6757f7363a9f | 288 | } |
Sissors | 0:6757f7363a9f | 289 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 290 | //-------------------Temperature-------------------- |
Sissors | 0:6757f7363a9f | 291 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 292 | int MPU6050::getTempRaw( void ) { |
Sissors | 0:6757f7363a9f | 293 | short retval; |
Sissors | 0:6757f7363a9f | 294 | char data[2]; |
Sissors | 0:6757f7363a9f | 295 | this->read(MPU6050_TEMP_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 296 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 297 | return (int)retval; |
Sissors | 0:6757f7363a9f | 298 | } |
Sissors | 0:6757f7363a9f | 299 | |
Sissors | 0:6757f7363a9f | 300 | float MPU6050::getTemp( void ) { |
Sissors | 0:6757f7363a9f | 301 | float retval; |
Sissors | 0:6757f7363a9f | 302 | retval=(float)this->getTempRaw(); |
Sissors | 0:6757f7363a9f | 303 | retval=(retval+521.0)/340.0+35.0; |
Sissors | 0:6757f7363a9f | 304 | return retval; |
Sissors | 0:6757f7363a9f | 305 | } |
Sissors | 0:6757f7363a9f | 306 |