wisnu pamungkas / MPU6050

mpu

Dependents:   SDFileSystem_mpu6050 Taller1

MPU6050.cpp@0:0e23b7f6dccd, 2014-02-20 (annotated)

Committer:
wisnup
Date:
Thu Feb 20 04:12:51 2014 +0000
Revision:
0:0e23b7f6dccd
mpu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wisnup 0:0e23b7f6dccd 1 /**
wisnup 0:0e23b7f6dccd 2 * Includes
wisnup 0:0e23b7f6dccd 3 */
wisnup 0:0e23b7f6dccd 4 #include "MPU6050.h"
wisnup 0:0e23b7f6dccd 5
wisnup 0:0e23b7f6dccd 6 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
wisnup 0:0e23b7f6dccd 7 this->setSleepMode(false);
wisnup 0:0e23b7f6dccd 8
wisnup 0:0e23b7f6dccd 9 //Initializations:
wisnup 0:0e23b7f6dccd 10 currentGyroRange = 0;
wisnup 0:0e23b7f6dccd 11 currentAcceleroRange=0;
wisnup 0:0e23b7f6dccd 12 }
wisnup 0:0e23b7f6dccd 13
wisnup 0:0e23b7f6dccd 14 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 15 //-------------------General------------------------
wisnup 0:0e23b7f6dccd 16 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 17
wisnup 0:0e23b7f6dccd 18 void MPU6050::write(char address, char data) {
wisnup 0:0e23b7f6dccd 19 char temp[2];
wisnup 0:0e23b7f6dccd 20 temp[0]=address;
wisnup 0:0e23b7f6dccd 21 temp[1]=data;
wisnup 0:0e23b7f6dccd 22
wisnup 0:0e23b7f6dccd 23 connection.write(MPU6050_ADDRESS * 2,temp,2);
wisnup 0:0e23b7f6dccd 24 }
wisnup 0:0e23b7f6dccd 25
wisnup 0:0e23b7f6dccd 26 char MPU6050::read(char address) {
wisnup 0:0e23b7f6dccd 27 char retval;
wisnup 0:0e23b7f6dccd 28 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
wisnup 0:0e23b7f6dccd 29 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
wisnup 0:0e23b7f6dccd 30 return retval;
wisnup 0:0e23b7f6dccd 31 }
wisnup 0:0e23b7f6dccd 32
wisnup 0:0e23b7f6dccd 33 void MPU6050::read(char address, char *data, int length) {
wisnup 0:0e23b7f6dccd 34 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
wisnup 0:0e23b7f6dccd 35 connection.read(MPU6050_ADDRESS * 2, data, length);
wisnup 0:0e23b7f6dccd 36 }
wisnup 0:0e23b7f6dccd 37
wisnup 0:0e23b7f6dccd 38 void MPU6050::setSleepMode(bool state) {
wisnup 0:0e23b7f6dccd 39 char temp;
wisnup 0:0e23b7f6dccd 40 temp = this->read(MPU6050_PWR_MGMT_1_REG);
wisnup 0:0e23b7f6dccd 41 if (state == true)
wisnup 0:0e23b7f6dccd 42 temp |= 1<<MPU6050_SLP_BIT;
wisnup 0:0e23b7f6dccd 43 if (state == false)
wisnup 0:0e23b7f6dccd 44 temp &= ~(1<<MPU6050_SLP_BIT);
wisnup 0:0e23b7f6dccd 45 this->write(MPU6050_PWR_MGMT_1_REG, temp);
wisnup 0:0e23b7f6dccd 46 }
wisnup 0:0e23b7f6dccd 47
wisnup 0:0e23b7f6dccd 48 bool MPU6050::testConnection( void ) {
wisnup 0:0e23b7f6dccd 49 char temp;
wisnup 0:0e23b7f6dccd 50 temp = this->read(MPU6050_WHO_AM_I_REG);
wisnup 0:0e23b7f6dccd 51 return (temp == (MPU6050_ADDRESS & 0xFE));
wisnup 0:0e23b7f6dccd 52 }
wisnup 0:0e23b7f6dccd 53
wisnup 0:0e23b7f6dccd 54 void MPU6050::setBW(char BW) {
wisnup 0:0e23b7f6dccd 55 char temp;
wisnup 0:0e23b7f6dccd 56 BW=BW & 0x07;
wisnup 0:0e23b7f6dccd 57 temp = this->read(MPU6050_CONFIG_REG);
wisnup 0:0e23b7f6dccd 58 temp &= 0xF8;
wisnup 0:0e23b7f6dccd 59 temp = temp + BW;
wisnup 0:0e23b7f6dccd 60 this->write(MPU6050_CONFIG_REG, temp);
wisnup 0:0e23b7f6dccd 61 }
wisnup 0:0e23b7f6dccd 62
wisnup 0:0e23b7f6dccd 63 void MPU6050::setI2CBypass(bool state) {
wisnup 0:0e23b7f6dccd 64 char temp;
wisnup 0:0e23b7f6dccd 65 temp = this->read(MPU6050_INT_PIN_CFG);
wisnup 0:0e23b7f6dccd 66 if (state == true)
wisnup 0:0e23b7f6dccd 67 temp |= 1<<MPU6050_BYPASS_BIT;
wisnup 0:0e23b7f6dccd 68 if (state == false)
wisnup 0:0e23b7f6dccd 69 temp &= ~(1<<MPU6050_BYPASS_BIT);
wisnup 0:0e23b7f6dccd 70 this->write(MPU6050_INT_PIN_CFG, temp);
wisnup 0:0e23b7f6dccd 71 }
wisnup 0:0e23b7f6dccd 72
wisnup 0:0e23b7f6dccd 73 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 74 //----------------Accelerometer---------------------
wisnup 0:0e23b7f6dccd 75 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 76
wisnup 0:0e23b7f6dccd 77 void MPU6050::setAcceleroRange( char range ) {
wisnup 0:0e23b7f6dccd 78 char temp;
wisnup 0:0e23b7f6dccd 79 range = range & 0x03;
wisnup 0:0e23b7f6dccd 80 currentAcceleroRange = range;
wisnup 0:0e23b7f6dccd 81
wisnup 0:0e23b7f6dccd 82 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
wisnup 0:0e23b7f6dccd 83 temp &= ~(3<<3);
wisnup 0:0e23b7f6dccd 84 temp = temp + (range<<3);
wisnup 0:0e23b7f6dccd 85 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
wisnup 0:0e23b7f6dccd 86 }
wisnup 0:0e23b7f6dccd 87
wisnup 0:0e23b7f6dccd 88 int MPU6050::getAcceleroRawX( void ) {
wisnup 0:0e23b7f6dccd 89 short retval;
wisnup 0:0e23b7f6dccd 90 char data[2];
wisnup 0:0e23b7f6dccd 91 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 92 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 93 return (int)retval;
wisnup 0:0e23b7f6dccd 94 }
wisnup 0:0e23b7f6dccd 95
wisnup 0:0e23b7f6dccd 96 int MPU6050::getAcceleroRawY( void ) {
wisnup 0:0e23b7f6dccd 97 short retval;
wisnup 0:0e23b7f6dccd 98 char data[2];
wisnup 0:0e23b7f6dccd 99 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 100 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 101 return (int)retval;
wisnup 0:0e23b7f6dccd 102 }
wisnup 0:0e23b7f6dccd 103
wisnup 0:0e23b7f6dccd 104 int MPU6050::getAcceleroRawZ( void ) {
wisnup 0:0e23b7f6dccd 105 short retval;
wisnup 0:0e23b7f6dccd 106 char data[2];
wisnup 0:0e23b7f6dccd 107 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 108 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 109 return (int)retval;
wisnup 0:0e23b7f6dccd 110 }
wisnup 0:0e23b7f6dccd 111
wisnup 0:0e23b7f6dccd 112 void MPU6050::getAcceleroRaw( int *data ) {
wisnup 0:0e23b7f6dccd 113 char temp[6];
wisnup 0:0e23b7f6dccd 114 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
wisnup 0:0e23b7f6dccd 115 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
wisnup 0:0e23b7f6dccd 116 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
wisnup 0:0e23b7f6dccd 117 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
wisnup 0:0e23b7f6dccd 118 }
wisnup 0:0e23b7f6dccd 119
wisnup 0:0e23b7f6dccd 120 void MPU6050::getAccelero( float *data ) {
wisnup 0:0e23b7f6dccd 121 int temp[3];
wisnup 0:0e23b7f6dccd 122 this->getAcceleroRaw(temp);
wisnup 0:0e23b7f6dccd 123 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
wisnup 0:0e23b7f6dccd 124 data[0]=(float)temp[0] / 16384.0 * 9.81;
wisnup 0:0e23b7f6dccd 125 data[1]=(float)temp[1] / 16384.0 * 9.81;
wisnup 0:0e23b7f6dccd 126 data[2]=(float)temp[2] / 16384.0 * 9.81;
wisnup 0:0e23b7f6dccd 127 }
wisnup 0:0e23b7f6dccd 128 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
wisnup 0:0e23b7f6dccd 129 data[0]=(float)temp[0] / 8192.0 * 9.81;
wisnup 0:0e23b7f6dccd 130 data[1]=(float)temp[1] / 8192.0 * 9.81;
wisnup 0:0e23b7f6dccd 131 data[2]=(float)temp[2] / 8192.0 * 9.81;
wisnup 0:0e23b7f6dccd 132 }
wisnup 0:0e23b7f6dccd 133 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
wisnup 0:0e23b7f6dccd 134 data[0]=(float)temp[0] / 4096.0 * 9.81;
wisnup 0:0e23b7f6dccd 135 data[1]=(float)temp[1] / 4096.0 * 9.81;
wisnup 0:0e23b7f6dccd 136 data[2]=(float)temp[2] / 4096.0 * 9.81;
wisnup 0:0e23b7f6dccd 137 }
wisnup 0:0e23b7f6dccd 138 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
wisnup 0:0e23b7f6dccd 139 data[0]=(float)temp[0] / 2048.0 * 9.81;
wisnup 0:0e23b7f6dccd 140 data[1]=(float)temp[1] / 2048.0 * 9.81;
wisnup 0:0e23b7f6dccd 141 data[2]=(float)temp[2] / 2048.0 * 9.81;
wisnup 0:0e23b7f6dccd 142 }
wisnup 0:0e23b7f6dccd 143
wisnup 0:0e23b7f6dccd 144 #ifdef DOUBLE_ACCELERO
wisnup 0:0e23b7f6dccd 145 data[0]*=2;
wisnup 0:0e23b7f6dccd 146 data[1]*=2;
wisnup 0:0e23b7f6dccd 147 data[2]*=2;
wisnup 0:0e23b7f6dccd 148 #endif
wisnup 0:0e23b7f6dccd 149 }
wisnup 0:0e23b7f6dccd 150
wisnup 0:0e23b7f6dccd 151 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 152 //------------------Gyroscope-----------------------
wisnup 0:0e23b7f6dccd 153 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 154 void MPU6050::setGyroRange( char range ) {
wisnup 0:0e23b7f6dccd 155 char temp;
wisnup 0:0e23b7f6dccd 156 currentGyroRange = range;
wisnup 0:0e23b7f6dccd 157 range = range & 0x03;
wisnup 0:0e23b7f6dccd 158 temp = this->read(MPU6050_GYRO_CONFIG_REG);
wisnup 0:0e23b7f6dccd 159 temp &= ~(3<<3);
wisnup 0:0e23b7f6dccd 160 temp = temp + range<<3;
wisnup 0:0e23b7f6dccd 161 this->write(MPU6050_GYRO_CONFIG_REG, temp);
wisnup 0:0e23b7f6dccd 162 }
wisnup 0:0e23b7f6dccd 163
wisnup 0:0e23b7f6dccd 164 int MPU6050::getGyroRawX( void ) {
wisnup 0:0e23b7f6dccd 165 short retval;
wisnup 0:0e23b7f6dccd 166 char data[2];
wisnup 0:0e23b7f6dccd 167 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 168 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 169 return (int)retval;
wisnup 0:0e23b7f6dccd 170 }
wisnup 0:0e23b7f6dccd 171
wisnup 0:0e23b7f6dccd 172 int MPU6050::getGyroRawY( void ) {
wisnup 0:0e23b7f6dccd 173 short retval;
wisnup 0:0e23b7f6dccd 174 char data[2];
wisnup 0:0e23b7f6dccd 175 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 176 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 177 return (int)retval;
wisnup 0:0e23b7f6dccd 178 }
wisnup 0:0e23b7f6dccd 179
wisnup 0:0e23b7f6dccd 180 int MPU6050::getGyroRawZ( void ) {
wisnup 0:0e23b7f6dccd 181 short retval;
wisnup 0:0e23b7f6dccd 182 char data[2];
wisnup 0:0e23b7f6dccd 183 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 184 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 185 return (int)retval;
wisnup 0:0e23b7f6dccd 186 }
wisnup 0:0e23b7f6dccd 187
wisnup 0:0e23b7f6dccd 188 void MPU6050::getGyroRaw( int *data ) {
wisnup 0:0e23b7f6dccd 189 char temp[6];
wisnup 0:0e23b7f6dccd 190 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
wisnup 0:0e23b7f6dccd 191 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
wisnup 0:0e23b7f6dccd 192 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
wisnup 0:0e23b7f6dccd 193 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
wisnup 0:0e23b7f6dccd 194 }
wisnup 0:0e23b7f6dccd 195
wisnup 0:0e23b7f6dccd 196 void MPU6050::getGyro( float *data ) {
wisnup 0:0e23b7f6dccd 197 int temp[3];
wisnup 0:0e23b7f6dccd 198 this->getGyroRaw(temp);
wisnup 0:0e23b7f6dccd 199 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
wisnup 0:0e23b7f6dccd 200 data[0]=(float)temp[0] / 7505.7;
wisnup 0:0e23b7f6dccd 201 data[1]=(float)temp[1] / 7505.7;
wisnup 0:0e23b7f6dccd 202 data[2]=(float)temp[2] / 7505.7;
wisnup 0:0e23b7f6dccd 203 }
wisnup 0:0e23b7f6dccd 204 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
wisnup 0:0e23b7f6dccd 205 data[0]=(float)temp[0] / 3752.9;
wisnup 0:0e23b7f6dccd 206 data[1]=(float)temp[1] / 3752.9;
wisnup 0:0e23b7f6dccd 207 data[2]=(float)temp[2] / 3752.9;
wisnup 0:0e23b7f6dccd 208 }
wisnup 0:0e23b7f6dccd 209 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
wisnup 0:0e23b7f6dccd 210 data[0]=(float)temp[0] / 1879.3;;
wisnup 0:0e23b7f6dccd 211 data[1]=(float)temp[1] / 1879.3;
wisnup 0:0e23b7f6dccd 212 data[2]=(float)temp[2] / 1879.3;
wisnup 0:0e23b7f6dccd 213 }
wisnup 0:0e23b7f6dccd 214 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
wisnup 0:0e23b7f6dccd 215 data[0]=(float)temp[0] / 939.7;
wisnup 0:0e23b7f6dccd 216 data[1]=(float)temp[1] / 939.7;
wisnup 0:0e23b7f6dccd 217 data[2]=(float)temp[2] / 939.7;
wisnup 0:0e23b7f6dccd 218 }
wisnup 0:0e23b7f6dccd 219 }
wisnup 0:0e23b7f6dccd 220 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 221 //-------------------Temperature--------------------
wisnup 0:0e23b7f6dccd 222 //--------------------------------------------------
wisnup 0:0e23b7f6dccd 223 int MPU6050::getTempRaw( void ) {
wisnup 0:0e23b7f6dccd 224 short retval;
wisnup 0:0e23b7f6dccd 225 char data[2];
wisnup 0:0e23b7f6dccd 226 this->read(MPU6050_TEMP_H_REG, data, 2);
wisnup 0:0e23b7f6dccd 227 retval = (data[0]<<8) + data[1];
wisnup 0:0e23b7f6dccd 228 return (int)retval;
wisnup 0:0e23b7f6dccd 229 }
wisnup 0:0e23b7f6dccd 230
wisnup 0:0e23b7f6dccd 231 float MPU6050::getTemp( void ) {
wisnup 0:0e23b7f6dccd 232 float retval;
wisnup 0:0e23b7f6dccd 233 retval=(float)this->getTempRaw();
wisnup 0:0e23b7f6dccd 234 retval=(retval+521.0)/340.0+35.0;
wisnup 0:0e23b7f6dccd 235 return retval;
wisnup 0:0e23b7f6dccd 236 }
wisnup 0:0e23b7f6dccd 237