marie allan / MPU6050

program test MPU6050

Dependents:   ticker_test_mpu6050

Fork of MPU6050 by Erik -

MPU6050.cpp@5:228ddc13168a, 2015-02-18 (annotated)

Committer:
allanmarie
Date:
Wed Feb 18 17:04:17 2015 +0000
Revision:
5:228ddc13168a
Parent:
4:4926b02b5eab 
modif MPU6050

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;
allanmarie 4:4926b02b5eab 12 MPU6050_ADDRESS=0x68;
allanmarie 5:228ddc13168a 13 //connection.frequency(400000); //mesures ne fonctionnent pas
allanmarie 4:4926b02b5eab 14 // test I2C connexion and update MPU6050_ADDRESS
allanmarie 4:4926b02b5eab 15 isFindMPU6050=I2C_finder();
allanmarie 4:4926b02b5eab 16 // if MPU6050 not found stop the program !!!!!
allanmarie 4:4926b02b5eab 17 if(!isFindMPU6050)
allanmarie 4:4926b02b5eab 18 {
allanmarie 4:4926b02b5eab 19 DigitalOut myled(LED1);
allanmarie 4:4926b02b5eab 20 printf("MPU6050 doesn't exist\n\r");
allanmarie 4:4926b02b5eab 21 printf("Check Connexion SCL and SDA and Power supply of your sensor\n\r");
allanmarie 4:4926b02b5eab 22 while(1) {
allanmarie 4:4926b02b5eab 23 myled = 1;
allanmarie 4:4926b02b5eab 24 wait(0.2);
allanmarie 4:4926b02b5eab 25 myled = 0;
allanmarie 4:4926b02b5eab 26 wait(0.2);
allanmarie 4:4926b02b5eab 27 }
allanmarie 4:4926b02b5eab 28 }
Sissors 0:6757f7363a9f 29 }
Sissors 0:6757f7363a9f 30
Sissors 0:6757f7363a9f 31 //--------------------------------------------------
Sissors 0:6757f7363a9f 32 //-------------------General------------------------
Sissors 0:6757f7363a9f 33 //--------------------------------------------------
Sissors 0:6757f7363a9f 34
allanmarie 4:4926b02b5eab 35 bool MPU6050::I2C_finder(){
allanmarie 4:4926b02b5eab 36 int i,isOK=1;
allanmarie 4:4926b02b5eab 37 char data;
allanmarie 4:4926b02b5eab 38 bool isFindSlave=false;
allanmarie 4:4926b02b5eab 39 printf("test I2C\n\r");
allanmarie 4:4926b02b5eab 40 for(i=1;i<127;i++)
allanmarie 4:4926b02b5eab 41 { wait(0.001);
allanmarie 4:4926b02b5eab 42 isOK=connection.read(i*2,&data,1,false);
allanmarie 4:4926b02b5eab 43 if(isOK==0){
allanmarie 4:4926b02b5eab 44 printf("slave I2C find : 0X%X\n\r",i);
allanmarie 4:4926b02b5eab 45 if(i==MPU6050_ADDRESS || i==MPU6050_ADDRESS+1){
allanmarie 4:4926b02b5eab 46 printf("MPU6050 has been found : 0x%X\n\r",i);
allanmarie 4:4926b02b5eab 47 MPU6050_ADDRESS=i; // on met l'adresse du MPU6050 = 0x68 ou 0x69
allanmarie 4:4926b02b5eab 48 isFindSlave=true;
allanmarie 4:4926b02b5eab 49 }
allanmarie 4:4926b02b5eab 50 }
allanmarie 4:4926b02b5eab 51 }
allanmarie 4:4926b02b5eab 52 return isFindSlave;
allanmarie 4:4926b02b5eab 53 }
allanmarie 4:4926b02b5eab 54
Sissors 0:6757f7363a9f 55 void MPU6050::write(char address, char data) {
Sissors 0:6757f7363a9f 56 char temp[2];
Sissors 0:6757f7363a9f 57 temp[0]=address;
Sissors 0:6757f7363a9f 58 temp[1]=data;
Sissors 0:6757f7363a9f 59
Sissors 0:6757f7363a9f 60 connection.write(MPU6050_ADDRESS * 2,temp,2);
Sissors 0:6757f7363a9f 61 }
Sissors 0:6757f7363a9f 62
Sissors 0:6757f7363a9f 63 char MPU6050::read(char address) {
Sissors 0:6757f7363a9f 64 char retval;
Sissors 0:6757f7363a9f 65 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Sissors 0:6757f7363a9f 66 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
Sissors 0:6757f7363a9f 67 return retval;
Sissors 0:6757f7363a9f 68 }
Sissors 0:6757f7363a9f 69
Sissors 0:6757f7363a9f 70 void MPU6050::read(char address, char *data, int length) {
Sissors 0:6757f7363a9f 71 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Sissors 0:6757f7363a9f 72 connection.read(MPU6050_ADDRESS * 2, data, length);
Sissors 0:6757f7363a9f 73 }
Sissors 0:6757f7363a9f 74
Sissors 0:6757f7363a9f 75 void MPU6050::setSleepMode(bool state) {
Sissors 0:6757f7363a9f 76 char temp;
Sissors 0:6757f7363a9f 77 temp = this->read(MPU6050_PWR_MGMT_1_REG);
Sissors 0:6757f7363a9f 78 if (state == true)
Sissors 0:6757f7363a9f 79 temp |= 1<<MPU6050_SLP_BIT;
Sissors 0:6757f7363a9f 80 if (state == false)
Sissors 0:6757f7363a9f 81 temp &= ~(1<<MPU6050_SLP_BIT);
Sissors 0:6757f7363a9f 82 this->write(MPU6050_PWR_MGMT_1_REG, temp);
Sissors 0:6757f7363a9f 83 }
Sissors 0:6757f7363a9f 84
Sissors 0:6757f7363a9f 85 bool MPU6050::testConnection( void ) {
allanmarie 4:4926b02b5eab 86 char temp=0;
allanmarie 4:4926b02b5eab 87 temp = this->read(MPU6050_WHO_AM_I_REG);
allanmarie 4:4926b02b5eab 88 printf(" WHO AM I : 0X%X\n\r",temp);
Sissors 0:6757f7363a9f 89 return (temp == (MPU6050_ADDRESS & 0xFE));
Sissors 0:6757f7363a9f 90 }
Sissors 0:6757f7363a9f 91
Sissors 0:6757f7363a9f 92 void MPU6050::setBW(char BW) {
Sissors 0:6757f7363a9f 93 char temp;
Sissors 0:6757f7363a9f 94 BW=BW & 0x07;
Sissors 0:6757f7363a9f 95 temp = this->read(MPU6050_CONFIG_REG);
Sissors 0:6757f7363a9f 96 temp &= 0xF8;
Sissors 0:6757f7363a9f 97 temp = temp + BW;
Sissors 0:6757f7363a9f 98 this->write(MPU6050_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 99 }
Sissors 0:6757f7363a9f 100
Sissors 0:6757f7363a9f 101 void MPU6050::setI2CBypass(bool state) {
Sissors 0:6757f7363a9f 102 char temp;
Sissors 0:6757f7363a9f 103 temp = this->read(MPU6050_INT_PIN_CFG);
Sissors 0:6757f7363a9f 104 if (state == true)
Sissors 0:6757f7363a9f 105 temp |= 1<<MPU6050_BYPASS_BIT;
Sissors 0:6757f7363a9f 106 if (state == false)
Sissors 0:6757f7363a9f 107 temp &= ~(1<<MPU6050_BYPASS_BIT);
Sissors 0:6757f7363a9f 108 this->write(MPU6050_INT_PIN_CFG, temp);
Sissors 0:6757f7363a9f 109 }
Sissors 0:6757f7363a9f 110
Sissors 0:6757f7363a9f 111 //--------------------------------------------------
Sissors 0:6757f7363a9f 112 //----------------Accelerometer---------------------
Sissors 0:6757f7363a9f 113 //--------------------------------------------------
Sissors 0:6757f7363a9f 114
Sissors 0:6757f7363a9f 115 void MPU6050::setAcceleroRange( char range ) {
Sissors 0:6757f7363a9f 116 char temp;
Sissors 0:6757f7363a9f 117 range = range & 0x03;
Sissors 0:6757f7363a9f 118 currentAcceleroRange = range;
Sissors 0:6757f7363a9f 119
Sissors 0:6757f7363a9f 120 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
Sissors 0:6757f7363a9f 121 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 122 temp = temp + (range<<3);
Sissors 0:6757f7363a9f 123 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 124 }
Sissors 0:6757f7363a9f 125
Sissors 0:6757f7363a9f 126 int MPU6050::getAcceleroRawX( void ) {
Sissors 0:6757f7363a9f 127 short retval;
Sissors 0:6757f7363a9f 128 char data[2];
Sissors 0:6757f7363a9f 129 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 130 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 131 return (int)retval;
Sissors 0:6757f7363a9f 132 }
Sissors 0:6757f7363a9f 133
Sissors 0:6757f7363a9f 134 int MPU6050::getAcceleroRawY( void ) {
Sissors 0:6757f7363a9f 135 short retval;
Sissors 0:6757f7363a9f 136 char data[2];
Sissors 0:6757f7363a9f 137 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 138 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 139 return (int)retval;
Sissors 0:6757f7363a9f 140 }
Sissors 0:6757f7363a9f 141
Sissors 0:6757f7363a9f 142 int MPU6050::getAcceleroRawZ( void ) {
Sissors 0:6757f7363a9f 143 short retval;
Sissors 0:6757f7363a9f 144 char data[2];
Sissors 0:6757f7363a9f 145 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 146 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 147 return (int)retval;
Sissors 0:6757f7363a9f 148 }
Sissors 0:6757f7363a9f 149
Sissors 0:6757f7363a9f 150 void MPU6050::getAcceleroRaw( int *data ) {
Sissors 0:6757f7363a9f 151 char temp[6];
Sissors 0:6757f7363a9f 152 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 153 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 154 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 155 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 156 }
Sissors 0:6757f7363a9f 157
Sissors 0:6757f7363a9f 158 void MPU6050::getAccelero( float *data ) {
Sissors 0:6757f7363a9f 159 int temp[3];
Sissors 0:6757f7363a9f 160 this->getAcceleroRaw(temp);
Sissors 0:6757f7363a9f 161 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
Sissors 0:6757f7363a9f 162 data[0]=(float)temp[0] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 163 data[1]=(float)temp[1] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 164 data[2]=(float)temp[2] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 165 }
Sissors 0:6757f7363a9f 166 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
Sissors 0:6757f7363a9f 167 data[0]=(float)temp[0] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 168 data[1]=(float)temp[1] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 169 data[2]=(float)temp[2] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 170 }
Sissors 0:6757f7363a9f 171 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
Sissors 0:6757f7363a9f 172 data[0]=(float)temp[0] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 173 data[1]=(float)temp[1] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 174 data[2]=(float)temp[2] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 175 }
Sissors 0:6757f7363a9f 176 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
Sissors 0:6757f7363a9f 177 data[0]=(float)temp[0] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 178 data[1]=(float)temp[1] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 179 data[2]=(float)temp[2] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 180 }
Sissors 0:6757f7363a9f 181
Sissors 0:6757f7363a9f 182 #ifdef DOUBLE_ACCELERO
Sissors 0:6757f7363a9f 183 data[0]*=2;
Sissors 0:6757f7363a9f 184 data[1]*=2;
Sissors 0:6757f7363a9f 185 data[2]*=2;
Sissors 0:6757f7363a9f 186 #endif
Sissors 0:6757f7363a9f 187 }
Sissors 0:6757f7363a9f 188
Sissors 0:6757f7363a9f 189 //--------------------------------------------------
Sissors 0:6757f7363a9f 190 //------------------Gyroscope-----------------------
Sissors 0:6757f7363a9f 191 //--------------------------------------------------
Sissors 0:6757f7363a9f 192 void MPU6050::setGyroRange( char range ) {
Sissors 0:6757f7363a9f 193 char temp;
Sissors 0:6757f7363a9f 194 currentGyroRange = range;
Sissors 0:6757f7363a9f 195 range = range & 0x03;
Sissors 0:6757f7363a9f 196 temp = this->read(MPU6050_GYRO_CONFIG_REG);
Sissors 0:6757f7363a9f 197 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 198 temp = temp + range<<3;
Sissors 0:6757f7363a9f 199 this->write(MPU6050_GYRO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 200 }
Sissors 0:6757f7363a9f 201
Sissors 0:6757f7363a9f 202 int MPU6050::getGyroRawX( void ) {
Sissors 0:6757f7363a9f 203 short retval;
Sissors 0:6757f7363a9f 204 char data[2];
Sissors 0:6757f7363a9f 205 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 206 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 207 return (int)retval;
Sissors 0:6757f7363a9f 208 }
Sissors 0:6757f7363a9f 209
Sissors 0:6757f7363a9f 210 int MPU6050::getGyroRawY( void ) {
Sissors 0:6757f7363a9f 211 short retval;
Sissors 0:6757f7363a9f 212 char data[2];
Sissors 0:6757f7363a9f 213 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 214 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 215 return (int)retval;
Sissors 0:6757f7363a9f 216 }
Sissors 0:6757f7363a9f 217
Sissors 0:6757f7363a9f 218 int MPU6050::getGyroRawZ( void ) {
Sissors 0:6757f7363a9f 219 short retval;
Sissors 0:6757f7363a9f 220 char data[2];
Sissors 0:6757f7363a9f 221 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 222 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 223 return (int)retval;
Sissors 0:6757f7363a9f 224 }
Sissors 0:6757f7363a9f 225
Sissors 0:6757f7363a9f 226 void MPU6050::getGyroRaw( int *data ) {
Sissors 0:6757f7363a9f 227 char temp[6];
Sissors 0:6757f7363a9f 228 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 229 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 230 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 231 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 232 }
Sissors 0:6757f7363a9f 233
Sissors 0:6757f7363a9f 234 void MPU6050::getGyro( float *data ) {
Sissors 0:6757f7363a9f 235 int temp[3];
Sissors 0:6757f7363a9f 236 this->getGyroRaw(temp);
Sissors 1:a3366f09e95c 237 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
Sissors 0:6757f7363a9f 238 data[0]=(float)temp[0] / 7505.7;
Sissors 0:6757f7363a9f 239 data[1]=(float)temp[1] / 7505.7;
Sissors 0:6757f7363a9f 240 data[2]=(float)temp[2] / 7505.7;
Sissors 0:6757f7363a9f 241 }
Sissors 1:a3366f09e95c 242 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
Sissors 0:6757f7363a9f 243 data[0]=(float)temp[0] / 3752.9;
Sissors 0:6757f7363a9f 244 data[1]=(float)temp[1] / 3752.9;
Sissors 0:6757f7363a9f 245 data[2]=(float)temp[2] / 3752.9;
Sissors 0:6757f7363a9f 246 }
Sissors 1:a3366f09e95c 247 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
Sissors 0:6757f7363a9f 248 data[0]=(float)temp[0] / 1879.3;;
Sissors 0:6757f7363a9f 249 data[1]=(float)temp[1] / 1879.3;
Sissors 0:6757f7363a9f 250 data[2]=(float)temp[2] / 1879.3;
Sissors 0:6757f7363a9f 251 }
Sissors 1:a3366f09e95c 252 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
Sissors 0:6757f7363a9f 253 data[0]=(float)temp[0] / 939.7;
Sissors 0:6757f7363a9f 254 data[1]=(float)temp[1] / 939.7;
Sissors 0:6757f7363a9f 255 data[2]=(float)temp[2] / 939.7;
Sissors 0:6757f7363a9f 256 }
Sissors 0:6757f7363a9f 257 }
Sissors 0:6757f7363a9f 258 //--------------------------------------------------
Sissors 0:6757f7363a9f 259 //-------------------Temperature--------------------
Sissors 0:6757f7363a9f 260 //--------------------------------------------------
Sissors 0:6757f7363a9f 261 int MPU6050::getTempRaw( void ) {
Sissors 0:6757f7363a9f 262 short retval;
Sissors 0:6757f7363a9f 263 char data[2];
Sissors 0:6757f7363a9f 264 this->read(MPU6050_TEMP_H_REG, data, 2);
Sissors 0:6757f7363a9f 265 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 266 return (int)retval;
Sissors 0:6757f7363a9f 267 }
Sissors 0:6757f7363a9f 268
Sissors 0:6757f7363a9f 269 float MPU6050::getTemp( void ) {
Sissors 0:6757f7363a9f 270 float retval;
Sissors 0:6757f7363a9f 271 retval=(float)this->getTempRaw();
Sissors 0:6757f7363a9f 272 retval=(retval+521.0)/340.0+35.0;
Sissors 0:6757f7363a9f 273 return retval;
Sissors 0:6757f7363a9f 274 }
Sissors 0:6757f7363a9f 275