![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Added for gyro and testing
MPU6050.cpp@6:502448484f91, 2015-02-12 (annotated)
- Committer:
- moklumbys
- Date:
- Thu Feb 12 21:01:38 2015 +0000
- Revision:
- 6:502448484f91
- Parent:
- 5:5873df1e58be
- Child:
- 7:56e591a74939
So, corrected all mistakes -> successfully built.
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 | } |
Sissors | 0:6757f7363a9f | 184 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 185 | //------------------Gyroscope----------------------- |
Sissors | 0:6757f7363a9f | 186 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 187 | void MPU6050::setGyroRange( char range ) { |
Sissors | 0:6757f7363a9f | 188 | char temp; |
Sissors | 0:6757f7363a9f | 189 | currentGyroRange = range; |
Sissors | 0:6757f7363a9f | 190 | range = range & 0x03; |
Sissors | 0:6757f7363a9f | 191 | temp = this->read(MPU6050_GYRO_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 192 | temp &= ~(3<<3); |
Sissors | 0:6757f7363a9f | 193 | temp = temp + range<<3; |
Sissors | 0:6757f7363a9f | 194 | this->write(MPU6050_GYRO_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 195 | } |
Sissors | 0:6757f7363a9f | 196 | |
Sissors | 0:6757f7363a9f | 197 | int MPU6050::getGyroRawX( void ) { |
Sissors | 0:6757f7363a9f | 198 | short retval; |
Sissors | 0:6757f7363a9f | 199 | char data[2]; |
Sissors | 0:6757f7363a9f | 200 | this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 201 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 202 | return (int)retval; |
Sissors | 0:6757f7363a9f | 203 | } |
Sissors | 0:6757f7363a9f | 204 | |
Sissors | 0:6757f7363a9f | 205 | int MPU6050::getGyroRawY( void ) { |
Sissors | 0:6757f7363a9f | 206 | short retval; |
Sissors | 0:6757f7363a9f | 207 | char data[2]; |
Sissors | 0:6757f7363a9f | 208 | this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 209 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 210 | return (int)retval; |
Sissors | 0:6757f7363a9f | 211 | } |
Sissors | 0:6757f7363a9f | 212 | |
Sissors | 0:6757f7363a9f | 213 | int MPU6050::getGyroRawZ( void ) { |
Sissors | 0:6757f7363a9f | 214 | short retval; |
Sissors | 0:6757f7363a9f | 215 | char data[2]; |
Sissors | 0:6757f7363a9f | 216 | this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 217 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 218 | return (int)retval; |
Sissors | 0:6757f7363a9f | 219 | } |
Sissors | 0:6757f7363a9f | 220 | |
Sissors | 0:6757f7363a9f | 221 | void MPU6050::getGyroRaw( int *data ) { |
Sissors | 0:6757f7363a9f | 222 | char temp[6]; |
Sissors | 0:6757f7363a9f | 223 | this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); |
Sissors | 0:6757f7363a9f | 224 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
Sissors | 0:6757f7363a9f | 225 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
Sissors | 0:6757f7363a9f | 226 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
Sissors | 0:6757f7363a9f | 227 | } |
Sissors | 0:6757f7363a9f | 228 | |
Sissors | 0:6757f7363a9f | 229 | void MPU6050::getGyro( float *data ) { |
Sissors | 0:6757f7363a9f | 230 | int temp[3]; |
Sissors | 0:6757f7363a9f | 231 | this->getGyroRaw(temp); |
Sissors | 1:a3366f09e95c | 232 | if (currentGyroRange == MPU6050_GYRO_RANGE_250) { |
moklumbys | 3:187152513f8d | 233 | data[0]=(float)temp[0] / 301.0; |
moklumbys | 3:187152513f8d | 234 | data[1]=(float)temp[1] / 301.0; |
moklumbys | 3:187152513f8d | 235 | data[2]=(float)temp[2] / 301.0; |
moklumbys | 3:187152513f8d | 236 | } //7505.5 |
Sissors | 1:a3366f09e95c | 237 | if (currentGyroRange == MPU6050_GYRO_RANGE_500){ |
Sissors | 0:6757f7363a9f | 238 | data[0]=(float)temp[0] / 3752.9; |
Sissors | 0:6757f7363a9f | 239 | data[1]=(float)temp[1] / 3752.9; |
Sissors | 0:6757f7363a9f | 240 | data[2]=(float)temp[2] / 3752.9; |
Sissors | 0:6757f7363a9f | 241 | } |
Sissors | 1:a3366f09e95c | 242 | if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ |
Sissors | 0:6757f7363a9f | 243 | data[0]=(float)temp[0] / 1879.3;; |
Sissors | 0:6757f7363a9f | 244 | data[1]=(float)temp[1] / 1879.3; |
Sissors | 0:6757f7363a9f | 245 | data[2]=(float)temp[2] / 1879.3; |
Sissors | 0:6757f7363a9f | 246 | } |
Sissors | 1:a3366f09e95c | 247 | if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ |
Sissors | 0:6757f7363a9f | 248 | data[0]=(float)temp[0] / 939.7; |
Sissors | 0:6757f7363a9f | 249 | data[1]=(float)temp[1] / 939.7; |
Sissors | 0:6757f7363a9f | 250 | data[2]=(float)temp[2] / 939.7; |
Sissors | 0:6757f7363a9f | 251 | } |
Sissors | 0:6757f7363a9f | 252 | } |
Sissors | 0:6757f7363a9f | 253 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 254 | //-------------------Temperature-------------------- |
Sissors | 0:6757f7363a9f | 255 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 256 | int MPU6050::getTempRaw( void ) { |
Sissors | 0:6757f7363a9f | 257 | short retval; |
Sissors | 0:6757f7363a9f | 258 | char data[2]; |
Sissors | 0:6757f7363a9f | 259 | this->read(MPU6050_TEMP_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 260 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 261 | return (int)retval; |
Sissors | 0:6757f7363a9f | 262 | } |
Sissors | 0:6757f7363a9f | 263 | |
Sissors | 0:6757f7363a9f | 264 | float MPU6050::getTemp( void ) { |
Sissors | 0:6757f7363a9f | 265 | float retval; |
Sissors | 0:6757f7363a9f | 266 | retval=(float)this->getTempRaw(); |
Sissors | 0:6757f7363a9f | 267 | retval=(retval+521.0)/340.0+35.0; |
Sissors | 0:6757f7363a9f | 268 | return retval; |
Sissors | 0:6757f7363a9f | 269 | } |
Sissors | 0:6757f7363a9f | 270 |