Avion Radio IUT / Mbed 2 deprecated MecatroPWM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MPU6050/MPU6050.cpp@3:3bc2882232a6, 2021-04-15 (annotated)

Committer:
qmaker
Date:
Thu Apr 15 08:43:24 2021 +0000
Revision:
3:3bc2882232a6
Parent:
0:0d257bbf5c05 
en cours de dev

Who changed what in which revision?

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