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