mpu
Dependents: SDFileSystem_mpu6050 Taller1
MPU6050.cpp
00001 /** 00002 * Includes 00003 */ 00004 #include "MPU6050.h" 00005 00006 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { 00007 this->setSleepMode(false); 00008 00009 //Initializations: 00010 currentGyroRange = 0; 00011 currentAcceleroRange=0; 00012 } 00013 00014 //-------------------------------------------------- 00015 //-------------------General------------------------ 00016 //-------------------------------------------------- 00017 00018 void MPU6050::write(char address, char data) { 00019 char temp[2]; 00020 temp[0]=address; 00021 temp[1]=data; 00022 00023 connection.write(MPU6050_ADDRESS * 2,temp,2); 00024 } 00025 00026 char MPU6050::read(char address) { 00027 char retval; 00028 connection.write(MPU6050_ADDRESS * 2, &address, 1, true); 00029 connection.read(MPU6050_ADDRESS * 2, &retval, 1); 00030 return retval; 00031 } 00032 00033 void MPU6050::read(char address, char *data, int length) { 00034 connection.write(MPU6050_ADDRESS * 2, &address, 1, true); 00035 connection.read(MPU6050_ADDRESS * 2, data, length); 00036 } 00037 00038 void MPU6050::setSleepMode(bool state) { 00039 char temp; 00040 temp = this->read(MPU6050_PWR_MGMT_1_REG); 00041 if (state == true) 00042 temp |= 1<<MPU6050_SLP_BIT; 00043 if (state == false) 00044 temp &= ~(1<<MPU6050_SLP_BIT); 00045 this->write(MPU6050_PWR_MGMT_1_REG, temp); 00046 } 00047 00048 bool MPU6050::testConnection( void ) { 00049 char temp; 00050 temp = this->read(MPU6050_WHO_AM_I_REG); 00051 return (temp == (MPU6050_ADDRESS & 0xFE)); 00052 } 00053 00054 void MPU6050::setBW(char BW) { 00055 char temp; 00056 BW=BW & 0x07; 00057 temp = this->read(MPU6050_CONFIG_REG); 00058 temp &= 0xF8; 00059 temp = temp + BW; 00060 this->write(MPU6050_CONFIG_REG, temp); 00061 } 00062 00063 void MPU6050::setI2CBypass(bool state) { 00064 char temp; 00065 temp = this->read(MPU6050_INT_PIN_CFG); 00066 if (state == true) 00067 temp |= 1<<MPU6050_BYPASS_BIT; 00068 if (state == false) 00069 temp &= ~(1<<MPU6050_BYPASS_BIT); 00070 this->write(MPU6050_INT_PIN_CFG, temp); 00071 } 00072 00073 //-------------------------------------------------- 00074 //----------------Accelerometer--------------------- 00075 //-------------------------------------------------- 00076 00077 void MPU6050::setAcceleroRange( char range ) { 00078 char temp; 00079 range = range & 0x03; 00080 currentAcceleroRange = range; 00081 00082 temp = this->read(MPU6050_ACCELERO_CONFIG_REG); 00083 temp &= ~(3<<3); 00084 temp = temp + (range<<3); 00085 this->write(MPU6050_ACCELERO_CONFIG_REG, temp); 00086 } 00087 00088 int MPU6050::getAcceleroRawX( void ) { 00089 short retval; 00090 char data[2]; 00091 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); 00092 retval = (data[0]<<8) + data[1]; 00093 return (int)retval; 00094 } 00095 00096 int MPU6050::getAcceleroRawY( void ) { 00097 short retval; 00098 char data[2]; 00099 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); 00100 retval = (data[0]<<8) + data[1]; 00101 return (int)retval; 00102 } 00103 00104 int MPU6050::getAcceleroRawZ( void ) { 00105 short retval; 00106 char data[2]; 00107 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); 00108 retval = (data[0]<<8) + data[1]; 00109 return (int)retval; 00110 } 00111 00112 void MPU6050::getAcceleroRaw( int *data ) { 00113 char temp[6]; 00114 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); 00115 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00116 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00117 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00118 } 00119 00120 void MPU6050::getAccelero( float *data ) { 00121 int temp[3]; 00122 this->getAcceleroRaw(temp); 00123 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { 00124 data[0]=(float)temp[0] / 16384.0 * 9.81; 00125 data[1]=(float)temp[1] / 16384.0 * 9.81; 00126 data[2]=(float)temp[2] / 16384.0 * 9.81; 00127 } 00128 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ 00129 data[0]=(float)temp[0] / 8192.0 * 9.81; 00130 data[1]=(float)temp[1] / 8192.0 * 9.81; 00131 data[2]=(float)temp[2] / 8192.0 * 9.81; 00132 } 00133 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ 00134 data[0]=(float)temp[0] / 4096.0 * 9.81; 00135 data[1]=(float)temp[1] / 4096.0 * 9.81; 00136 data[2]=(float)temp[2] / 4096.0 * 9.81; 00137 } 00138 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ 00139 data[0]=(float)temp[0] / 2048.0 * 9.81; 00140 data[1]=(float)temp[1] / 2048.0 * 9.81; 00141 data[2]=(float)temp[2] / 2048.0 * 9.81; 00142 } 00143 00144 #ifdef DOUBLE_ACCELERO 00145 data[0]*=2; 00146 data[1]*=2; 00147 data[2]*=2; 00148 #endif 00149 } 00150 00151 //-------------------------------------------------- 00152 //------------------Gyroscope----------------------- 00153 //-------------------------------------------------- 00154 void MPU6050::setGyroRange( char range ) { 00155 char temp; 00156 currentGyroRange = range; 00157 range = range & 0x03; 00158 temp = this->read(MPU6050_GYRO_CONFIG_REG); 00159 temp &= ~(3<<3); 00160 temp = temp + range<<3; 00161 this->write(MPU6050_GYRO_CONFIG_REG, temp); 00162 } 00163 00164 int MPU6050::getGyroRawX( void ) { 00165 short retval; 00166 char data[2]; 00167 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); 00168 retval = (data[0]<<8) + data[1]; 00169 return (int)retval; 00170 } 00171 00172 int MPU6050::getGyroRawY( void ) { 00173 short retval; 00174 char data[2]; 00175 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); 00176 retval = (data[0]<<8) + data[1]; 00177 return (int)retval; 00178 } 00179 00180 int MPU6050::getGyroRawZ( void ) { 00181 short retval; 00182 char data[2]; 00183 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); 00184 retval = (data[0]<<8) + data[1]; 00185 return (int)retval; 00186 } 00187 00188 void MPU6050::getGyroRaw( int *data ) { 00189 char temp[6]; 00190 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); 00191 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00192 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00193 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00194 } 00195 00196 void MPU6050::getGyro( float *data ) { 00197 int temp[3]; 00198 this->getGyroRaw(temp); 00199 if (currentGyroRange == MPU6050_GYRO_RANGE_250) { 00200 data[0]=(float)temp[0] / 7505.7; 00201 data[1]=(float)temp[1] / 7505.7; 00202 data[2]=(float)temp[2] / 7505.7; 00203 } 00204 if (currentGyroRange == MPU6050_GYRO_RANGE_500){ 00205 data[0]=(float)temp[0] / 3752.9; 00206 data[1]=(float)temp[1] / 3752.9; 00207 data[2]=(float)temp[2] / 3752.9; 00208 } 00209 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ 00210 data[0]=(float)temp[0] / 1879.3;; 00211 data[1]=(float)temp[1] / 1879.3; 00212 data[2]=(float)temp[2] / 1879.3; 00213 } 00214 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ 00215 data[0]=(float)temp[0] / 939.7; 00216 data[1]=(float)temp[1] / 939.7; 00217 data[2]=(float)temp[2] / 939.7; 00218 } 00219 } 00220 //-------------------------------------------------- 00221 //-------------------Temperature-------------------- 00222 //-------------------------------------------------- 00223 int MPU6050::getTempRaw( void ) { 00224 short retval; 00225 char data[2]; 00226 this->read(MPU6050_TEMP_H_REG, data, 2); 00227 retval = (data[0]<<8) + data[1]; 00228 return (int)retval; 00229 } 00230 00231 float MPU6050::getTemp( void ) { 00232 float retval; 00233 retval=(float)this->getTempRaw(); 00234 retval=(retval+521.0)/340.0+35.0; 00235 return retval; 00236 } 00237
Generated on Thu Jul 14 2022 03:27:03 by 1.7.2