Diff: MPU.h

Revision:

0:54b67cd15a5b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU.h	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,173 @@
+// 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