Panusorn Chinsakuljaroen / Mbed 2 deprecated BTL432kc

imu for l432kc

Dependencies:   mbed

MPU6050.cpp@4:7b04752df27f, 2018-12-08 (annotated)

Committer:
sunninety1
Date:
Sat Dec 08 05:48:54 2018 +0000
Revision:
4:7b04752df27f
Parent:
2:01ca44dd3908 
for l432kc;

Who changed what in which revision?

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