File content as of revision 10:5bdd3dfd5f59:

float gx,gy,gz,ax,ay,az; //deg/s, cm/s^2  (x-vert, y-forward, z-left)
#include "MPU6050.h" //Rewrite !!!

MPU6050 gyro(PB_9,PB_8);

float gyro_x, gyro_y, gyro_angle, gyro_v, gyro_s; //cm, deg

void gyro_init(){
    gyro.getID(); //dummy read (first transmission is bad)
    //pc.printf("%hhu ",gyro.getID());
    gyro.start();
    }

void gyro_process(){ // reading - 500 uS  // doesn't work in interrupt
    gyro.read(&gx,&gy,&gz,&ax,&ay,&az);
    
    //wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az);
    //wifi.printf("%.4f %.2f ",ay,gz);

    gyro_v -= 0.05*100*ay; //0.05 -> realtime_step //inertial_navigation
    gyro_s += 0.05*gyro_v;
    gyro_angle += 0.05*gx;
    gyro_x += 0.05*gyro_v*sin(gyro_angle);
    gyro_y += 0.05*gyro_v*cos(gyro_angle); //sqr(1-x^2)
    
    //wifi.printf("v:%.1f, s:%.1f, deg:%.1f, x:%.1f, y:%.1f\n",gyro_v,gyro_s,gyro_angle,gyro_x,gyro_y);
}

void gyro_balance(){ //?????
    //gyro_read_ay();
    }