cebri alyac / MPU6050

yes

Dependents:   Asservissement_Gyro

MPU6050.cpp@0:2b5072b2ee48, 2022-05-08 (annotated)

Committer:
braichi13
Date:
Sun May 08 14:40:15 2022 +0000
Revision:
0:2b5072b2ee48
Yees

Who changed what in which revision?

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