File content as of revision 0:06770dc62477:

//==================================================
//Atitude estimation (compensated filter)
//MPU board: mbed LPC1768
//Accelerometer +Gyro sensor : GY-521
//2019/10/11 A.Toda
//==================================================
#include "mbed.h"
#include "MPU6050.h"

//==================================================
#define RAD_TO_DEG          57.2957795f             // 180 / π
#define MAX_MEAN_COUNTER 100
#define ACC_X 1.3//offset of x-axi accelerometer  
//==================================================

//Timer interrupt
Ticker flipper;

//Port Setting
MPU6050 mpu(p9, p10);  //Accelerometer + Gyro
                        //(SDA,SCLK)

Serial pc(USBTX, USBRX);    //UART

//==================================================
//Accelerometer and gyro data
//==================================================
double acc[3]; //variables for accelerometer
double gyro[3]; //variables for gyro

double offset_gyro_x=0.0;
double offset_gyro_y=0.0;

double sum_gyro_x=0.0;
double sum_gyro_y=0.0;

double threshold_acc,threshold_acc_ini;

//==================================================
//Atitude data
//==================================================
double roll_and_pitch_acc[2];//atitude from acceleromter
double roll_and_pitch[2];//atitude from gyro and acceleromter

//==================================================
//Gyro and accelerometer functions
//==================================================
//get data
void  aquisition_sensor_values(double *a,double *g){
    
    float ac[3],gy[3];
    
    mpu.getAccelero(ac);//get acceleration (Accelerometer)
                                //x_axis acc[0]
                                //y_axis acc[1]
                                //z_axis acc[2]
    mpu.getGyro(gy);   //get rate of angle(Gyro)
                      //x_axis gyro[0]
                      //y_axis gyro[1]
                      //z_axis gyro[2]
        
    //Invertion for direction of Accelerometer axis
    ac[0]*=(-1.0);
    ac[0]+=ACC_X;
    
    ac[2]*=(-1.0);
        
    //Unit convertion of rate of angle(radian to degree)
    gy[0]*=RAD_TO_DEG;
    gy[0]*=(-1.0);
        
    gy[1]*=RAD_TO_DEG;        
        
    gy[2]*=RAD_TO_DEG;
    gy[2]*=(-1.0);
  
    for(int i=0;i<3;i++){
        a[i]=double(ac[i]);
        g[i]=double(gy[i]);
        }
    g[0]-=offset_gyro_x;//offset rejection
    g[1]-=offset_gyro_y;//offset rejection
    
    return;
    
}

//calculate offset of gyro
void offset_calculation_for_gyro(){
    
    //Accelerometer and gyro setting 
    mpu.setAcceleroRange(0);//acceleration range is +-2G
    mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
    
    //calculate offset of gyro
    for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
        aquisition_sensor_values(acc,gyro);
        sum_gyro_x+=gyro[0];
        sum_gyro_y+=gyro[1];
        wait(0.01);
        }
    
    offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
    offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
    
    return;
}

//atitude calculation from acceleromter
void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
    
    //roll_and_pitch[0] = atan2(a[1], a[2])*RAD_TO_DEG;//roll
    roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
    //roll_and_pitch[1] = atan(a[0]/sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch
    roll_and_pitch[1] = atan2(a[0],sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch
            
    return;
}

//atitude calculation
void atitude_update(){
    
    aquisition_sensor_values(acc,gyro);
    
    roll_and_pitch[0]+=gyro[0]*0.01;
    roll_and_pitch[1]+=gyro[1]*0.01;
    
    threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    
    if((threshold_acc>=0.9*threshold_acc_ini)
          &&(threshold_acc<=1.1*threshold_acc_ini)){
        
        atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
        roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
        //roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
        roll_and_pitch[1] = roll_and_pitch_acc[1];
        
        }else{}
    
    pc.printf("roll=%f pitch=%f\r\n",roll_and_pitch[0],roll_and_pitch[1]);
    
    
    return;

}

//==================================================
//Main
//==================================================
int main() {
    
    //UART initialization
    pc.baud(115200);
    
    //gyro and accelerometer initialization
    offset_calculation_for_gyro();
    
    //identify initilal atitude
    aquisition_sensor_values(acc,gyro);
    atitude_estimation_from_accelerometer(acc,roll_and_pitch);
    
    threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    
    //Ticker
    flipper.attach(&atitude_update, 0.01);
    
    //while
    while(1) {

        wait(0.01);
    }
}