MPU6050.cpp

00001 #include "MPU6050.h"
00002 
00003 MPU6050::MPU6050(){
00004    
00005    Panic=false;
00006 }
00007 
00008 void MPU6050::write(char reg,char data){
00009     
00010     char tx[2]={reg,data};
00011 
00012     I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, tx, 2);
00013 
00014  }
00015 
00016 int MPU6050::read (char reg){
00017 
00018     char tx = reg;
00019     char rx[2];
00020 
00021     I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
00022 
00023     I2C0.blockread((MPU_ADDRESS << 1)|0x01, rx, 2);
00024 
00025     int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
00026     return output;
00027 }
00028 
00029 bool MPU6050::CheckConnection(void){
00030      
00031     char tx = 0x75; // Who Am I Register 
00032     char rx;
00033 
00034     I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
00035 
00036     I2C0.blockread((MPU_ADDRESS << 1)|0x01, &rx, 1);
00037     
00038     if(rx==0x68) return true;
00039         
00040         else return false ;
00041 }
00042 
00043 void MPU6050::MPU_Setup(void){
00044 
00045        write(0x6B,0x80); // Restart 
00046        wait_ms(5);
00047        write(0x6B,0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) 
00048        write(0x1B,0x18); //GYRO_CONFIG   -- FS_SEL =3 : Scale = +/-2000 deg/sec
00049        write(0x1C,0x10); //ACCEL_CONFIG  -- AFS_SEL= 2 : Scale = +/-8G
00050        write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth 
00051        wait_ms(60);
00052 }
00053 
00054 void MPU6050::ScaledGyro(void){
00055    
00056     Scaled_GyroX = -(float)read(0x45)/GyroScale;
00057     Scaled_GyroY =  (float)read(0x43)/GyroScale;
00058     Scaled_GyroZ =  (float)read(0x47)/GyroScale;
00059 }
00060 
00061 void MPU6050::ScaledAcc(void){
00062    
00063    Scaled_AccX = (float)read(0x3B)/AccScale;
00064    Scaled_AccY = (float)read(0x3D)/AccScale;
00065    Scaled_AccZ = (float)read(0x3F)/AccScale;
00066 }
00067 
00068 void MPU6050::RawAcc(void){
00069    
00070    Raw_AccX = read(0x3B);
00071    Raw_AccY = read(0x3D);
00072    Raw_AccZ = read(0x3F);
00073 }
00074 
00075  void MPU6050::CalibrateGyro(void){
00076  
00077      for(int k=0;k<100;k++){
00078      ScaledGyro();
00079      GyroOffset[0]+=Scaled_GyroX;
00080      GyroOffset[1]+=Scaled_GyroY;
00081      GyroOffset[2]+=Scaled_GyroZ;
00082      wait_ms(5);
00083      }
00084      GyroOffset[0]/=100;
00085      GyroOffset[1]/=100;
00086      GyroOffset[2]/=100;
00087 
00088  }
00089  
00090   void MPU6050::CalibrateAcc(void){
00091  
00092      for(int k=0;k<100;k++){
00093      ScaledAcc();
00094      AccOffset[0]+=Scaled_AccX;
00095      AccOffset[1]+=Scaled_AccY;
00096      AccOffset[2]+=Scaled_AccZ;
00097      wait_ms(5);
00098      }
00099      AccOffset[0]/=100;
00100      AccOffset[1]/=100;
00101      AccOffset[2]/=100;
00102      AccOffset[2]-=1;
00103      
00104  }
00105  
00106  void MPU6050::GyroRate(void){
00107     
00108     ScaledGyro();
00109     Scaled_GyroX-=GyroOffset[0];
00110     Scaled_GyroY-=GyroOffset[1];
00111     Scaled_GyroZ-=GyroOffset[2];
00112  }
00113  
00114  void MPU6050::Acc(void){
00115  
00116     ScaledAcc();
00117     Scaled_AccX-=AccOffset[0];
00118     Scaled_AccY-=AccOffset[1];
00119     Scaled_AccZ-=AccOffset[2];
00120      
00121  }
00122  
00123    void MPU6050::filterGyro(void){
00124       
00125        GyroRate();
00126     
00127     // Filter X Axis //
00128     filtered_Gyro[0] = filter_GyroX.update(Scaled_GyroX);
00129     
00130     //Filter Y Axis //
00131     filtered_Gyro[1] = filter_GyroY.update(Scaled_GyroY);
00132     
00133     // Filter Z Axis //
00134     filtered_Gyro[2] = filter_GyroZ.update(Scaled_GyroZ);
00135  }
00136    
00137    
00138  void MPU6050::filterAcc(void){
00139       
00140        Acc();
00141     
00142     // Filter X Axis//
00143     filtered_Acc[0] = filter_AccX.update(Scaled_AccX);
00144     
00145     // Filter Y Axis //
00146     filtered_Acc[1] = filter_AccY.update(Scaled_AccY);
00147     
00148     // Filter Z Axis//
00149     filtered_Acc[2] = filter_AccZ.update(Scaled_AccZ);
00150     
00151     Acceleration_Magnitude = sqrt(filtered_Acc[0]*filtered_Acc[0] + filtered_Acc[1]*filtered_Acc[1] + filtered_Acc[2]*filtered_Acc[2]);
00152     
00153     if(Acceleration_Magnitude==0) Panic = true; // Free-Fall detection;
00154     else Panic= false;
00155  }
00156  
00157   void MPU6050::AccAngle(void){
00158   
00159    //filterAcc();
00160    Acc();
00161    
00162    //accangle[0] = ToDeg(atan2(filtered_Acc[0],sqrt(filtered_Acc[1]*filtered_Acc[1]+filtered_Acc[2]*filtered_Acc[2])));
00163    accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
00164    //accangle[1] = ToDeg(atan2(filtered_Acc[1],sqrt(filtered_Acc[0]*filtered_Acc[0]+filtered_Acc[2]*filtered_Acc[2])));
00165    accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
00166  }
00167