Library containing functions to talk with both the MPU6050 IMUs on the Limbic Controller.
Fork of MPU6050 by
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); 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, temp); 00046 } 00047 00048 char MPU6050::testConnection( void ) { 00049 char temp; 00050 temp = this->read(MPU6050_WHO_AM_I); 00051 //return (temp == (MPU6050_ADDRESS & 0xFE)); 00052 return temp; 00053 } 00054 00055 void MPU6050::setBW(char BW) { 00056 char temp; 00057 BW=BW & 0x07; 00058 temp = this->read(MPU6050_CONFIG); 00059 temp &= 0xF8; 00060 temp = temp + BW; 00061 this->write(MPU6050_CONFIG, temp); 00062 } 00063 00064 void MPU6050::setI2CBypass(bool state) { 00065 char temp; 00066 temp = this->read(MPU6050_INT_PIN_CFG); 00067 if (state == true) 00068 temp |= 1<<MPU6050_BYPASS_BIT; 00069 if (state == false) 00070 temp &= ~(1<<MPU6050_BYPASS_BIT); 00071 this->write(MPU6050_INT_PIN_CFG, temp); 00072 } 00073 00074 //-------------------------------------------------- 00075 //----------------Accelerometer--------------------- 00076 //-------------------------------------------------- 00077 00078 void MPU6050::setAcceleroRange( char range ) { 00079 char temp; 00080 range = range & 0x03; 00081 currentAcceleroRange = range; 00082 00083 temp = this->read(MPU6050_ACCEL_CONFIG); 00084 temp &= ~(3<<3); 00085 temp = temp + (range<<3); 00086 this->write(MPU6050_ACCEL_CONFIG, temp); 00087 } 00088 00089 int MPU6050::getAcceleroRawX( void ) { 00090 short retval; 00091 char data[2]; 00092 this->read(MPU6050_ACCEL_XOUT_H, data, 2); 00093 retval = (data[0]<<8) + data[1]; 00094 return (int)retval; 00095 } 00096 00097 int MPU6050::getAcceleroRawY( void ) { 00098 short retval; 00099 char data[2]; 00100 this->read(MPU6050_ACCEL_YOUT_H, data, 2); 00101 retval = (data[0]<<8) + data[1]; 00102 return (int)retval; 00103 } 00104 00105 int MPU6050::getAcceleroRawZ( void ) { 00106 short retval; 00107 char data[2]; 00108 this->read(MPU6050_ACCEL_ZOUT_H, data, 2); 00109 retval = (data[0]<<8) + data[1]; 00110 return (int)retval; 00111 } 00112 00113 void MPU6050::getAcceleroRaw( int *data ) { 00114 char temp[6]; 00115 this->read(MPU6050_ACCEL_XOUT_H, temp, 6); 00116 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00117 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00118 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00119 } 00120 00121 void MPU6050::getAccelero( float *data ) { 00122 int temp[3]; 00123 this->getAcceleroRaw(temp); 00124 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { 00125 data[0]=(float)temp[0] / 16384.0 * 9.81; 00126 data[1]=(float)temp[1] / 16384.0 * 9.81; 00127 data[2]=(float)temp[2] / 16384.0 * 9.81; 00128 } 00129 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ 00130 data[0]=(float)temp[0] / 8192.0 * 9.81; 00131 data[1]=(float)temp[1] / 8192.0 * 9.81; 00132 data[2]=(float)temp[2] / 8192.0 * 9.81; 00133 } 00134 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ 00135 data[0]=(float)temp[0] / 4096.0 * 9.81; 00136 data[1]=(float)temp[1] / 4096.0 * 9.81; 00137 data[2]=(float)temp[2] / 4096.0 * 9.81; 00138 } 00139 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ 00140 data[0]=(float)temp[0] / 2048.0 * 9.81; 00141 data[1]=(float)temp[1] / 2048.0 * 9.81; 00142 data[2]=(float)temp[2] / 2048.0 * 9.81; 00143 } 00144 00145 #ifdef DOUBLE_ACCELERO 00146 data[0]*=2; 00147 data[1]*=2; 00148 data[2]*=2; 00149 #endif 00150 } 00151 00152 //-------------------------------------------------- 00153 //------------------Gyroscope----------------------- 00154 //-------------------------------------------------- 00155 void MPU6050::setGyroRange( char range ) { 00156 char temp; 00157 currentGyroRange = range; 00158 range = range & 0x03; 00159 temp = this->read(MPU6050_GYRO_CONFIG); 00160 temp &= ~(3<<3); 00161 temp = temp + range<<3; 00162 this->write(MPU6050_GYRO_CONFIG, temp); 00163 } 00164 00165 int MPU6050::getGyroRawX( void ) { 00166 short retval; 00167 char data[2]; 00168 this->read(MPU6050_GYRO_XOUT_H, data, 2); 00169 retval = (data[0]<<8) + data[1]; 00170 return (int)retval; 00171 } 00172 00173 int MPU6050::getGyroRawY( void ) { 00174 short retval; 00175 char data[2]; 00176 this->read(MPU6050_GYRO_YOUT_H, data, 2); 00177 retval = (data[0]<<8) + data[1]; 00178 return (int)retval; 00179 } 00180 00181 int MPU6050::getGyroRawZ( void ) { 00182 short retval; 00183 char data[2]; 00184 this->read(MPU6050_GYRO_ZOUT_H, data, 2); 00185 retval = (data[0]<<8) + data[1]; 00186 return (int)retval; 00187 } 00188 00189 void MPU6050::getGyroRaw( int *data ) { 00190 char temp[6]; 00191 this->read(MPU6050_GYRO_XOUT_H, temp, 6); 00192 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00193 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00194 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00195 } 00196 00197 void MPU6050::getGyro( float *data ) { 00198 int temp[3]; 00199 this->getGyroRaw(temp); 00200 if (currentGyroRange == MPU6050_GYRO_RANGE_250) { 00201 data[0]=(float)temp[0] / 7505.7; 00202 data[1]=(float)temp[1] / 7505.7; 00203 data[2]=(float)temp[2] / 7505.7; 00204 } 00205 if (currentGyroRange == MPU6050_GYRO_RANGE_500){ 00206 data[0]=(float)temp[0] / 3752.9; 00207 data[1]=(float)temp[1] / 3752.9; 00208 data[2]=(float)temp[2] / 3752.9; 00209 } 00210 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ 00211 data[0]=(float)temp[0] / 1879.3;; 00212 data[1]=(float)temp[1] / 1879.3; 00213 data[2]=(float)temp[2] / 1879.3; 00214 } 00215 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ 00216 data[0]=(float)temp[0] / 939.7; 00217 data[1]=(float)temp[1] / 939.7; 00218 data[2]=(float)temp[2] / 939.7; 00219 } 00220 } 00221 //-------------------------------------------------- 00222 //-------------------Temperature-------------------- 00223 //-------------------------------------------------- 00224 int MPU6050::getTempRaw( void ) { 00225 short retval; 00226 char data[2]; 00227 this->read(MPU6050_TEMP_OUT_H, data, 2); 00228 retval = (data[0]<<8) + data[1]; 00229 return (int)retval; 00230 } 00231 00232 float MPU6050::getTemp( void ) { 00233 float retval; 00234 retval=(float)this->getTempRaw(); 00235 retval=(retval+521.0)/340.0+35.0; 00236 return retval; 00237 }
Generated on Sat Jul 16 2022 08:52:51 by 1.7.2