File content as of revision 0:54b67cd15a5b:

// 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)));
 }