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