Timothy Daw / Mbed 2 deprecated main2

mpu6050

Dependencies:   mbed MPU6050

MPU6050.cpp@0:68bfb6ebd7a1, 2020-02-28 (annotated)

Committer:
TimothyDaw
Date:
Fri Feb 28 18:48:19 2020 +0000
Revision:
0:68bfb6ebd7a1
mpu6050

Who changed what in which revision?

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