rau cha / Mbed 2 deprecated MPU6050-test-wihoutLib

For test MPU6050

Dependencies:   mbed

MPU6050.cpp@0:897889ddd75a, 2014-07-08 (annotated)

Committer:
Shikaneo
Date:
Tue Jul 08 05:38:58 2014 +0000
Revision:
0:897889ddd75a
first commit

Who changed what in which revision?

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