Diff: MPU.h

Revision:

1:e08a4f517989

Parent:

0:54b67cd15a5b

--- a/MPU.h	Sun Dec 23 23:50:21 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,173 +0,0 @@
-// Contains all Sensor related functions : initializing the sensor, calibrating sensors, getting data , accelerometer angles , and filtering gyro/acc data 
-
-#define MPU_ADDRESS 0x68
-#define GyroScale  16.4
-#define AccScale   4096
-
-I2C i2c(p9,p10);
-
-float filtered_AccX[4],filtered_AccY[4],filtered_AccZ[4],filtered_GyroX[4],filtered_GyroY[4],filtered_GyroZ[4]; 
-float Scaled_GyroX,Scaled_GyroY,Scaled_GyroZ;
-float Scaled_AccX,Scaled_AccY,Scaled_AccZ;
-float GyroOffset[3],AccOffset[3];
-float accangle[2];
-
-void write(char reg,char data){
-    
-    char tx[2]={reg,data};
-    
-    i2c.write((MPU_ADDRESS << 1)&0xFE, tx, 2);
-    
- }
-
-int read (char reg){
-
-    char tx = reg;
-    char rx[2];
-    
-    i2c.write((MPU_ADDRESS << 1)&0xFE , &tx,1);
-    
-    i2c.read((MPU_ADDRESS << 1)|0x01, rx, 2);
-    
-    
-    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
- 
-    return output;
-}
-
-void MPU_Setup(){
-       
-       
-       write(0x6B,0x80); // Restart 
-       wait_ms(5);
-       write(0x6B,0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 1 (PLL with Z Gyro reference) 
-       write(0x1B,0x18); //GYRO_CONFIG   -- FS_SEL = 2 : 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 ScaledGyro(){
-   
-    Scaled_GyroX = -read(0x45)/GyroScale;
-    Scaled_GyroY =  read(0x43)/GyroScale;
-    Scaled_GyroZ =  read(0x47)/GyroScale;
-}
-
-void ScaledAcc(){
-   
-   Scaled_AccX = (float)read(0x3B)/AccScale;
-   Scaled_AccY = (float)read(0x3D)/AccScale;
-   Scaled_AccZ = (float)read(0x3F)/AccScale;
-}
-
- void CalibrateGyro(){
- 
-     for(int k=0;k<200;k++){
-     ScaledGyro();
-     GyroOffset[0]+=Scaled_GyroX;
-     GyroOffset[1]+=Scaled_GyroY;
-     GyroOffset[2]+=Scaled_GyroZ;
-     wait_ms(1);
-     }
-     GyroOffset[0]/=200;
-     GyroOffset[1]/=200;
-     GyroOffset[2]/=200;
- }
- 
-  void CalibrateAcc(){
- 
-     for(int k=0;k<200;k++){
-     ScaledAcc();
-     AccOffset[0]+=Scaled_AccX;
-     AccOffset[1]+=Scaled_AccY;
-     AccOffset[2]+=Scaled_AccZ;
-     wait_ms(1);
-     }
-     AccOffset[0]/=200;
-     AccOffset[1]/=200;
-     AccOffset[2]/=200;
-     AccOffset[2]-=1;
- }
- 
- void GyroRate(){
-    
-    ScaledGyro();
-    Scaled_GyroX-=GyroOffset[0];
-    Scaled_GyroY-=GyroOffset[1];
-    Scaled_GyroZ-=GyroOffset[2];
- }
- 
- void Acc(){
- 
-    ScaledAcc();
-    Scaled_AccX-=AccOffset[0];
-    Scaled_AccY-=AccOffset[1];
-    Scaled_AccZ-=AccOffset[2];
- }
-   void filterGyro(void){
-      
-       GyroRate();
-    
-    //////////////////////////////////////// Filter X Axis ////////////////////////////////////////////////////////////////
-    filtered_GyroX[0] = Scaled_GyroX;
-    Scaled_GyroX      = filtered_GyroX[0]*0.25 + filtered_GyroX[1]*0.25 + filtered_GyroX[2]*0.25 + filtered_GyroX[3]*0.25;
-    filtered_GyroX[3] = filtered_GyroX[2];
-    filtered_GyroX[2] = filtered_GyroX[1];
-    filtered_GyroX[1] = filtered_GyroX[0];
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    /////////////////////////////////////// Filter Y Axis ////////////////////////////////////////////////////////////////
-    filtered_GyroY[0] = Scaled_GyroY;
-    Scaled_GyroY      = filtered_GyroY[0]*0.25 + filtered_GyroY[1]*0.25 + filtered_GyroY[2]*0.25 + filtered_GyroY[3]*0.25;
-    filtered_GyroY[3] = filtered_GyroY[2];
-    filtered_GyroY[2] = filtered_GyroY[1];
-    filtered_GyroY[1] = filtered_GyroY[0];
-    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    /////////////////////////////////////// Filter Z Axis ////////////////////////////////////////////////////////////////
-    filtered_GyroZ[0] = Scaled_GyroZ;
-    Scaled_GyroZ      = filtered_GyroZ[0]*0.25 + filtered_GyroZ[1]*0.25 + filtered_GyroZ[2]*0.25 + filtered_GyroZ[3]*0.25;
-    filtered_GyroZ[3] = filtered_GyroZ[2];
-    filtered_GyroZ[2] = filtered_GyroZ[1];
-    filtered_GyroZ[1] = filtered_GyroZ[0];
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }
-   
-   
- void filterAcc(void){
-      
-       Acc();
-    
-    //////////////////////////////////////// Filter X Axis///////////////////////////////////////////////////////////
-    filtered_AccX[0] = Scaled_AccX;
-    Scaled_AccX      = filtered_AccX[0]*0.25 + filtered_AccX[1]*0.25 + filtered_AccX[2]*0.25 + filtered_AccX[3]*0.25;
-    filtered_AccX[3] = filtered_AccX[2];
-    filtered_AccX[2] = filtered_AccX[1];
-    filtered_AccX[1] = filtered_AccX[0];
-    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    //////////////////////////////////////// Filter Y Axis //////////////////////////////////////////////////////////
-    filtered_AccY[0] = Scaled_AccY;
-    Scaled_AccY      = filtered_AccY[0]*0.25 + filtered_AccY[1]*0.25 + filtered_AccY[2]*0.25 + filtered_AccY[3]*0.25;
-    filtered_AccY[3] = filtered_AccY[2];
-    filtered_AccY[2] = filtered_AccY[1];
-    filtered_AccY[1] = filtered_AccY[0];
-    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    //////////////////////////////////////// Filter Z Axis//////////////////////////////////////////////////////////
-    filtered_AccZ[0] = Scaled_AccZ;
-    Scaled_AccZ      = filtered_AccZ[0]*0.25 + filtered_AccZ[1]*0.25 + filtered_AccZ[2]*0.25 + filtered_AccZ[3]*0.25;
-    filtered_AccZ[3] = filtered_AccZ[2];
-    filtered_AccZ[2] = filtered_AccZ[1];
-    filtered_AccZ[1] = filtered_AccZ[0];
-    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }
- 
-  void AccAngle(){
-  
-   filterAcc();
-   
-   accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
-   accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
- }
\ No newline at end of file