File content as of revision 2:01ca44dd3908:

#include "mbed.h"
#include "MPU6050.h"
#include "math.h"
MPU6050 mpu(D4,D5);
Serial pc(USBTX, USBRX); // tx, rx
Timer t;
Serial  serial(PA_9,PA_10);
char    data[30];

//DigitalIn res(USER_BUTTON);

float ark[3];
    float gy[3];
    
    int timestamp=0;
    float Now,lastupdate;
float cali_ax,cali_ay,cali_az,cali_gx,cali_gy,cali_gz;
    static float v[3]={0,0,0};
    
float PI = 3.14159265358979323846f;
void MPUcalibate(){
    static float axx=0,ayy=0,azz=0,gxx=0,gyy=0,gzz=0;
    int x=0;
    while(x<=9999) {
        mpu.getAccelero(ark);
        mpu.getGyro(gy);
        axx+=ark[0];
        ayy+=ark[1];
        azz+=ark[2];
        gxx+=gy[0];
        gyy+=gy[1];
        gzz+=gy[2];
        x+=1;
        wait(0.0001);
    }
    cali_ax=axx/10000.0f;
    cali_ay=ayy/10000.0f;
    cali_az=azz/10000.0f;
    cali_gx=gxx/10000.0f;
    cali_gy=gyy/10000.0f;
    cali_gz=gzz/10000.0f;
    //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay,cali_az-9.80665);
    //pc.printf("%.10f %.10f %.10f\n",cali_ax-9.80665,cali_ay,cali_az);
    //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay-9.80665,cali_az);
    //pc.printf("%f %f %f\n",cali_gx,cali_gy,cali_gz);
    }
void AccelToVelo(float ax,float ay,float az,float dt){
    v[0]+=(ax*dt/129); //(-156.10655212402344+150.74868774414062);
    v[1]+=(ay*dt/180); //(-208.04095458984375+194.97883605957031);
    v[2]+=(az*dt/6); //(151.52879333496094-144.20004272460938);
    }
/*float CalculateAngle(float ax,float ay,float az,float gx,float gy,float gz){
    float pitch;
    gx=0;
    gy=0;
    gz=0;
    static float fillter_pitch=0;
    fillter_pitch=(fillter_pitch*0.8)+(pitch*0.2);
    pitch=atan((ax)/sqrt((ay)*(ay)+(az)*(az))*(180/PI));
    //pc.printf("%f\n",pitch);
    return fillter_pitch;
    }*/
int main()
{
    pc.baud(115200);
    static float calibate_acc[3]={0.6465099045, 0.14739338841, -0.2634094528};
    static float calibate_gy[3]={-0.022125, 0.005254 ,-0.009761};
    mpu.getAccelero(ark);
        mpu.getGyro(gy);
    //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
    static float fillter_pitch=0;
    int co=0;
    float pitch;
    float delay=0.0001,current=0.0,deltat;
    t.start();
    
    wait(0.5);
    
    //MPUcalibate();
    //cali_err=atan((-(ark[1]-cali_ay))/(ark[2]-cali_az));
    //cali_p=atan2(((ark[2]-cali_az)),(ark[0]-cali_ax));
    //cali_p=atan((ark[0]-cali_ax)/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)));
    //pc.printf("cali P %f \n",cali_err);
    //wait(1);
      /*  fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
    fillter_pitch3=(fillter_pitch3*0.75)+(fillter_pitch2*0.25);
    fillter_pitch4=(fillter_pitch4*0.75)+(fillter_pitch3*0.25);
    fillter_pitch5=(fillter_pitch5*0.75)+(fillter_pitch4*0.25);
    fillter_pitch6=(fillter_pitch6*0.75)+(fillter_pitch5*0.25);
    fillter_pitch7=(fillter_pitch7*0.75)+(fillter_pitch6*0.25);
    fillter_pitch8=(fillter_pitch8*0.75)+(fillter_pitch7*0.25);
    fillter_pitch9=(fillter_pitch9*0.75)+(fillter_pitch8*0.25);
    fillter_pitch10=(fillter_pitch10*0.75)+(fillter_pitch9*0.25);*/
    while(1) {
        Now = t.read();
        deltat=Now-current;
        if (deltat>0.1){
        mpu.getAccelero(ark);
        mpu.getGyro(gy);
        ark[0] -= calibate_acc[0];
        ark[1] -= calibate_acc[1];
        ark[2] -= calibate_acc[2];
        gy[0] -= calibate_gy[0];
        gy[1] -= calibate_gy[1];
        gy[2] -= calibate_gy[2];
        float g=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
        while(co<1000){
        AccelToVelo((ark[0]),ark[1],ark[2]-9.8065,deltat);
        co+=1;
        }
        co=0;
        ark[0]+=0.5;
        float gra=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
        pc.printf("Q AX %f AY %f AZ %f G %f \n",ark[0],ark[1],ark[2],gra);
        if(serial.writeable()) {
            serial.printf("%f %f %f %f \n",ark[0],ark[1],ark[2],gra);
            //serial.printf("------------------------ \n");
            //serial.printf("   Gyx:%f_Gyy:%f_Gyz:%f \n",gy[0],gy[1],gy[2]);
            wait_ms(500);   // lag to debounce the button
    }
        //pc.printf("Q GyX %f GyY %f GyZ %f \n",gy[0],gy[1],gy[2]);
        //pc.printf("Q Vx %f Vy %f Vz %f \n",v[0],v[1],v[2]);
        //pc.printf("%.14f\n",v[1]/100);
        pc.printf("Q AX %f AY %f AZ %f GyX %f GyY %f GyZ %f \n",ark[0]+0.5,ark[1],ark[2],gy[0],gy[1],gy[2]);
        //timestamp+=1;
        //pc.printf("%f\n",atan((-(ark[1]-cali_ay))/(ark[2]-cali_az)));
        //pc.printf("Ax %f\n",(atan2((ark[2]-cali_az),(ark[0]-cali_ax))-cali_p)*(180/PI));
        //pitch=atan((ark[0])/sqrt((ark[1])*(ark[1])+(ark[2])*ark[2]))*(180/PI);
        //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
        //pc.printf("%lf\n",CalculateAngle(ark[0],ark[1],ark[2],0,0,0));
        pitch=atan(ark[0]/sqrt(pow(ark[1],2)+pow(ark[2],2)))*(180/PI);
    //fillter_pitch=(fillter_pitch*0.75)+(pitch*0.25);
   // fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
   // pc.printf("%f\n",fillter_pitch);
        //pc.printf("Ax %f\n",(atan((ark[0])/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)))-cali_p)*(180/PI));
       // pc.printf("%.10f %.10f %.10f\n",ark[0],ark[1],ark[2]);
        //pc.printf("%f %f %f\n",gy[0],gy[1],gy[2]);
        //pc.printf("%.2f\n",t.read());
        wait(0.02);
        /*if (res==0) {
            //lastupdate=t.read()-Now;
            //Now=lastupdate;
            t.reset();
        }*/
current=Now;
}
    }
}