Mark van Raai / MPU6050

Library containing functions to talk with both the MPU6050 IMUs on the Limbic Controller.

Fork of MPU6050 by Mark Vandermeulen

MPU6050.cpp@4:d1262cdbcf9d, 2016-04-05 (annotated)

Committer:
zork
Date:
Tue Apr 05 16:19:06 2016 +0000
Revision:
4:d1262cdbcf9d
Parent:
3:16cb051b0702 
Mouse changed to absolute;

Who changed what in which revision?

UserRevisionLine numberNew 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;
zork 4:d1262cdbcf9d 40 temp = this->read(MPU6050_PWR_MGMT_1);
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);
zork 4:d1262cdbcf9d 45 this->write(MPU6050_PWR_MGMT_1, temp);
Sissors 0:6757f7363a9f 46 }
Sissors 0:6757f7363a9f 47
majik 3:16cb051b0702 48 char MPU6050::testConnection( void ) {
Sissors 0:6757f7363a9f 49 char temp;
zork 4:d1262cdbcf9d 50 temp = this->read(MPU6050_WHO_AM_I);
majik 3:16cb051b0702 51 //return (temp == (MPU6050_ADDRESS & 0xFE));
majik 3:16cb051b0702 52 return temp;
Sissors 0:6757f7363a9f 53 }
Sissors 0:6757f7363a9f 54
Sissors 0:6757f7363a9f 55 void MPU6050::setBW(char BW) {
Sissors 0:6757f7363a9f 56 char temp;
Sissors 0:6757f7363a9f 57 BW=BW & 0x07;
zork 4:d1262cdbcf9d 58 temp = this->read(MPU6050_CONFIG);
Sissors 0:6757f7363a9f 59 temp &= 0xF8;
Sissors 0:6757f7363a9f 60 temp = temp + BW;
zork 4:d1262cdbcf9d 61 this->write(MPU6050_CONFIG, temp);
Sissors 0:6757f7363a9f 62 }
Sissors 0:6757f7363a9f 63
Sissors 0:6757f7363a9f 64 void MPU6050::setI2CBypass(bool state) {
Sissors 0:6757f7363a9f 65 char temp;
Sissors 0:6757f7363a9f 66 temp = this->read(MPU6050_INT_PIN_CFG);
Sissors 0:6757f7363a9f 67 if (state == true)
Sissors 0:6757f7363a9f 68 temp |= 1<<MPU6050_BYPASS_BIT;
Sissors 0:6757f7363a9f 69 if (state == false)
Sissors 0:6757f7363a9f 70 temp &= ~(1<<MPU6050_BYPASS_BIT);
Sissors 0:6757f7363a9f 71 this->write(MPU6050_INT_PIN_CFG, temp);
Sissors 0:6757f7363a9f 72 }
Sissors 0:6757f7363a9f 73
Sissors 0:6757f7363a9f 74 //--------------------------------------------------
Sissors 0:6757f7363a9f 75 //----------------Accelerometer---------------------
Sissors 0:6757f7363a9f 76 //--------------------------------------------------
Sissors 0:6757f7363a9f 77
Sissors 0:6757f7363a9f 78 void MPU6050::setAcceleroRange( char range ) {
Sissors 0:6757f7363a9f 79 char temp;
Sissors 0:6757f7363a9f 80 range = range & 0x03;
Sissors 0:6757f7363a9f 81 currentAcceleroRange = range;
Sissors 0:6757f7363a9f 82
zork 4:d1262cdbcf9d 83 temp = this->read(MPU6050_ACCEL_CONFIG);
Sissors 0:6757f7363a9f 84 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 85 temp = temp + (range<<3);
zork 4:d1262cdbcf9d 86 this->write(MPU6050_ACCEL_CONFIG, temp);
Sissors 0:6757f7363a9f 87 }
Sissors 0:6757f7363a9f 88
Sissors 0:6757f7363a9f 89 int MPU6050::getAcceleroRawX( void ) {
Sissors 0:6757f7363a9f 90 short retval;
Sissors 0:6757f7363a9f 91 char data[2];
zork 4:d1262cdbcf9d 92 this->read(MPU6050_ACCEL_XOUT_H, data, 2);
Sissors 0:6757f7363a9f 93 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 94 return (int)retval;
Sissors 0:6757f7363a9f 95 }
Sissors 0:6757f7363a9f 96
Sissors 0:6757f7363a9f 97 int MPU6050::getAcceleroRawY( void ) {
Sissors 0:6757f7363a9f 98 short retval;
Sissors 0:6757f7363a9f 99 char data[2];
zork 4:d1262cdbcf9d 100 this->read(MPU6050_ACCEL_YOUT_H, data, 2);
Sissors 0:6757f7363a9f 101 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 102 return (int)retval;
Sissors 0:6757f7363a9f 103 }
Sissors 0:6757f7363a9f 104
Sissors 0:6757f7363a9f 105 int MPU6050::getAcceleroRawZ( void ) {
Sissors 0:6757f7363a9f 106 short retval;
Sissors 0:6757f7363a9f 107 char data[2];
zork 4:d1262cdbcf9d 108 this->read(MPU6050_ACCEL_ZOUT_H, data, 2);
Sissors 0:6757f7363a9f 109 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 110 return (int)retval;
Sissors 0:6757f7363a9f 111 }
Sissors 0:6757f7363a9f 112
Sissors 0:6757f7363a9f 113 void MPU6050::getAcceleroRaw( int *data ) {
Sissors 0:6757f7363a9f 114 char temp[6];
zork 4:d1262cdbcf9d 115 this->read(MPU6050_ACCEL_XOUT_H, temp, 6);
Sissors 0:6757f7363a9f 116 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 117 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 118 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 119 }
Sissors 0:6757f7363a9f 120
Sissors 0:6757f7363a9f 121 void MPU6050::getAccelero( float *data ) {
Sissors 0:6757f7363a9f 122 int temp[3];
Sissors 0:6757f7363a9f 123 this->getAcceleroRaw(temp);
Sissors 0:6757f7363a9f 124 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
Sissors 0:6757f7363a9f 125 data[0]=(float)temp[0] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 126 data[1]=(float)temp[1] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 127 data[2]=(float)temp[2] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 128 }
Sissors 0:6757f7363a9f 129 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
Sissors 0:6757f7363a9f 130 data[0]=(float)temp[0] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 131 data[1]=(float)temp[1] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 132 data[2]=(float)temp[2] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 133 }
Sissors 0:6757f7363a9f 134 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
Sissors 0:6757f7363a9f 135 data[0]=(float)temp[0] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 136 data[1]=(float)temp[1] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 137 data[2]=(float)temp[2] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 138 }
Sissors 0:6757f7363a9f 139 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
Sissors 0:6757f7363a9f 140 data[0]=(float)temp[0] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 141 data[1]=(float)temp[1] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 142 data[2]=(float)temp[2] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 143 }
Sissors 0:6757f7363a9f 144
Sissors 0:6757f7363a9f 145 #ifdef DOUBLE_ACCELERO
Sissors 0:6757f7363a9f 146 data[0]*=2;
Sissors 0:6757f7363a9f 147 data[1]*=2;
Sissors 0:6757f7363a9f 148 data[2]*=2;
Sissors 0:6757f7363a9f 149 #endif
Sissors 0:6757f7363a9f 150 }
Sissors 0:6757f7363a9f 151
Sissors 0:6757f7363a9f 152 //--------------------------------------------------
Sissors 0:6757f7363a9f 153 //------------------Gyroscope-----------------------
Sissors 0:6757f7363a9f 154 //--------------------------------------------------
Sissors 0:6757f7363a9f 155 void MPU6050::setGyroRange( char range ) {
Sissors 0:6757f7363a9f 156 char temp;
Sissors 0:6757f7363a9f 157 currentGyroRange = range;
Sissors 0:6757f7363a9f 158 range = range & 0x03;
zork 4:d1262cdbcf9d 159 temp = this->read(MPU6050_GYRO_CONFIG);
Sissors 0:6757f7363a9f 160 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 161 temp = temp + range<<3;
zork 4:d1262cdbcf9d 162 this->write(MPU6050_GYRO_CONFIG, temp);
Sissors 0:6757f7363a9f 163 }
Sissors 0:6757f7363a9f 164
Sissors 0:6757f7363a9f 165 int MPU6050::getGyroRawX( void ) {
Sissors 0:6757f7363a9f 166 short retval;
Sissors 0:6757f7363a9f 167 char data[2];
zork 4:d1262cdbcf9d 168 this->read(MPU6050_GYRO_XOUT_H, data, 2);
Sissors 0:6757f7363a9f 169 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 170 return (int)retval;
Sissors 0:6757f7363a9f 171 }
Sissors 0:6757f7363a9f 172
Sissors 0:6757f7363a9f 173 int MPU6050::getGyroRawY( void ) {
Sissors 0:6757f7363a9f 174 short retval;
Sissors 0:6757f7363a9f 175 char data[2];
zork 4:d1262cdbcf9d 176 this->read(MPU6050_GYRO_YOUT_H, data, 2);
Sissors 0:6757f7363a9f 177 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 178 return (int)retval;
Sissors 0:6757f7363a9f 179 }
Sissors 0:6757f7363a9f 180
Sissors 0:6757f7363a9f 181 int MPU6050::getGyroRawZ( void ) {
Sissors 0:6757f7363a9f 182 short retval;
Sissors 0:6757f7363a9f 183 char data[2];
zork 4:d1262cdbcf9d 184 this->read(MPU6050_GYRO_ZOUT_H, data, 2);
Sissors 0:6757f7363a9f 185 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 186 return (int)retval;
Sissors 0:6757f7363a9f 187 }
Sissors 0:6757f7363a9f 188
Sissors 0:6757f7363a9f 189 void MPU6050::getGyroRaw( int *data ) {
Sissors 0:6757f7363a9f 190 char temp[6];
zork 4:d1262cdbcf9d 191 this->read(MPU6050_GYRO_XOUT_H, temp, 6);
Sissors 0:6757f7363a9f 192 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 193 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 194 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 195 }
Sissors 0:6757f7363a9f 196
Sissors 0:6757f7363a9f 197 void MPU6050::getGyro( float *data ) {
Sissors 0:6757f7363a9f 198 int temp[3];
Sissors 0:6757f7363a9f 199 this->getGyroRaw(temp);
Sissors 1:a3366f09e95c 200 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
Sissors 0:6757f7363a9f 201 data[0]=(float)temp[0] / 7505.7;
Sissors 0:6757f7363a9f 202 data[1]=(float)temp[1] / 7505.7;
Sissors 0:6757f7363a9f 203 data[2]=(float)temp[2] / 7505.7;
Sissors 0:6757f7363a9f 204 }
Sissors 1:a3366f09e95c 205 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
Sissors 0:6757f7363a9f 206 data[0]=(float)temp[0] / 3752.9;
Sissors 0:6757f7363a9f 207 data[1]=(float)temp[1] / 3752.9;
Sissors 0:6757f7363a9f 208 data[2]=(float)temp[2] / 3752.9;
Sissors 0:6757f7363a9f 209 }
Sissors 1:a3366f09e95c 210 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
Sissors 0:6757f7363a9f 211 data[0]=(float)temp[0] / 1879.3;;
Sissors 0:6757f7363a9f 212 data[1]=(float)temp[1] / 1879.3;
Sissors 0:6757f7363a9f 213 data[2]=(float)temp[2] / 1879.3;
Sissors 0:6757f7363a9f 214 }
Sissors 1:a3366f09e95c 215 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
Sissors 0:6757f7363a9f 216 data[0]=(float)temp[0] / 939.7;
Sissors 0:6757f7363a9f 217 data[1]=(float)temp[1] / 939.7;
Sissors 0:6757f7363a9f 218 data[2]=(float)temp[2] / 939.7;
Sissors 0:6757f7363a9f 219 }
Sissors 0:6757f7363a9f 220 }
Sissors 0:6757f7363a9f 221 //--------------------------------------------------
Sissors 0:6757f7363a9f 222 //-------------------Temperature--------------------
Sissors 0:6757f7363a9f 223 //--------------------------------------------------
Sissors 0:6757f7363a9f 224 int MPU6050::getTempRaw( void ) {
Sissors 0:6757f7363a9f 225 short retval;
Sissors 0:6757f7363a9f 226 char data[2];
zork 4:d1262cdbcf9d 227 this->read(MPU6050_TEMP_OUT_H, data, 2);
Sissors 0:6757f7363a9f 228 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 229 return (int)retval;
Sissors 0:6757f7363a9f 230 }
Sissors 0:6757f7363a9f 231
Sissors 0:6757f7363a9f 232 float MPU6050::getTemp( void ) {
Sissors 0:6757f7363a9f 233 float retval;
Sissors 0:6757f7363a9f 234 retval=(float)this->getTempRaw();
Sissors 0:6757f7363a9f 235 retval=(retval+521.0)/340.0+35.0;
Sissors 0:6757f7363a9f 236 return retval;
Sissors 0:6757f7363a9f 237 }