Diff: MPU/MPU6050.cpp

Revision:

1:e08a4f517989

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU/MPU6050.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,167 @@
+#include "MPU6050.h"
+
+MPU6050::MPU6050(){
+   
+   Panic=false;
+}
+
+void MPU6050::write(char reg,char data){
+    
+    char tx[2]={reg,data};
+
+    I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, tx, 2);
+
+ }
+
+int MPU6050::read (char reg){
+
+    char tx = reg;
+    char rx[2];
+
+    I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
+
+    I2C0.blockread((MPU_ADDRESS << 1)|0x01, rx, 2);
+
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+    return output;
+}
+
+bool MPU6050::CheckConnection(void){
+     
+    char tx = 0x75; // Who Am I Register 
+    char rx;
+
+    I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
+
+    I2C0.blockread((MPU_ADDRESS << 1)|0x01, &rx, 1);
+    
+    if(rx==0x68) return true;
+        
+        else return false ;
+}
+
+void MPU6050::MPU_Setup(void){
+
+       write(0x6B,0x80); // Restart 
+       wait_ms(5);
+       write(0x6B,0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) 
+       write(0x1B,0x18); //GYRO_CONFIG   -- FS_SEL =3 : Scale = +/-2000 deg/sec
+       write(0x1C,0x10); //ACCEL_CONFIG  -- AFS_SEL= 2 : Scale = +/-8G
+       write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth 
+       wait_ms(60);
+}
+
+void MPU6050::ScaledGyro(void){
+   
+    Scaled_GyroX = -(float)read(0x45)/GyroScale;
+    Scaled_GyroY =  (float)read(0x43)/GyroScale;
+    Scaled_GyroZ =  (float)read(0x47)/GyroScale;
+}
+
+void MPU6050::ScaledAcc(void){
+   
+   Scaled_AccX = (float)read(0x3B)/AccScale;
+   Scaled_AccY = (float)read(0x3D)/AccScale;
+   Scaled_AccZ = (float)read(0x3F)/AccScale;
+}
+
+void MPU6050::RawAcc(void){
+   
+   Raw_AccX = read(0x3B);
+   Raw_AccY = read(0x3D);
+   Raw_AccZ = read(0x3F);
+}
+
+ void MPU6050::CalibrateGyro(void){
+ 
+     for(int k=0;k<100;k++){
+     ScaledGyro();
+     GyroOffset[0]+=Scaled_GyroX;
+     GyroOffset[1]+=Scaled_GyroY;
+     GyroOffset[2]+=Scaled_GyroZ;
+     wait_ms(5);
+     }
+     GyroOffset[0]/=100;
+     GyroOffset[1]/=100;
+     GyroOffset[2]/=100;
+
+ }
+ 
+  void MPU6050::CalibrateAcc(void){
+ 
+     for(int k=0;k<100;k++){
+     ScaledAcc();
+     AccOffset[0]+=Scaled_AccX;
+     AccOffset[1]+=Scaled_AccY;
+     AccOffset[2]+=Scaled_AccZ;
+     wait_ms(5);
+     }
+     AccOffset[0]/=100;
+     AccOffset[1]/=100;
+     AccOffset[2]/=100;
+     AccOffset[2]-=1;
+     
+ }
+ 
+ void MPU6050::GyroRate(void){
+    
+    ScaledGyro();
+    Scaled_GyroX-=GyroOffset[0];
+    Scaled_GyroY-=GyroOffset[1];
+    Scaled_GyroZ-=GyroOffset[2];
+ }
+ 
+ void MPU6050::Acc(void){
+ 
+    ScaledAcc();
+    Scaled_AccX-=AccOffset[0];
+    Scaled_AccY-=AccOffset[1];
+    Scaled_AccZ-=AccOffset[2];
+     
+ }
+ 
+   void MPU6050::filterGyro(void){
+      
+       GyroRate();
+    
+    // Filter X Axis //
+    filtered_Gyro[0] = filter_GyroX.update(Scaled_GyroX);
+    
+    //Filter Y Axis //
+    filtered_Gyro[1] = filter_GyroY.update(Scaled_GyroY);
+    
+    // Filter Z Axis //
+    filtered_Gyro[2] = filter_GyroZ.update(Scaled_GyroZ);
+ }
+   
+   
+ void MPU6050::filterAcc(void){
+      
+       Acc();
+    
+    // Filter X Axis//
+    filtered_Acc[0] = filter_AccX.update(Scaled_AccX);
+    
+    // Filter Y Axis //
+    filtered_Acc[1] = filter_AccY.update(Scaled_AccY);
+    
+    // Filter Z Axis//
+    filtered_Acc[2] = filter_AccZ.update(Scaled_AccZ);
+    
+    Acceleration_Magnitude = sqrt(filtered_Acc[0]*filtered_Acc[0] + filtered_Acc[1]*filtered_Acc[1] + filtered_Acc[2]*filtered_Acc[2]);
+    
+    if(Acceleration_Magnitude==0) Panic = true; // Free-Fall detection;
+    else Panic= false;
+ }
+ 
+  void MPU6050::AccAngle(void){
+  
+   //filterAcc();
+   Acc();
+   
+   //accangle[0] = ToDeg(atan2(filtered_Acc[0],sqrt(filtered_Acc[1]*filtered_Acc[1]+filtered_Acc[2]*filtered_Acc[2])));
+   accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
+   //accangle[1] = ToDeg(atan2(filtered_Acc[1],sqrt(filtered_Acc[0]*filtered_Acc[0]+filtered_Acc[2]*filtered_Acc[2])));
+   accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
+ }
+ 
\ No newline at end of file