gyro.h

00001 float gx,gy,gz,ax,ay,az; //deg/s, m/s^2  (x-vert, y-forward, z-left)
00002 #include "MPU6050.h" //Rewrite !!!
00003 
00004 MPU6050 gyro(PB_9,PB_8);
00005 
00006 float gyro_x, gyro_y, gyro_angle, gyro_v, gyro_s; //cm, deg
00007 
00008 void gyro_init(){
00009     gyro.getID(); //dummy read (first transmission is bad)
00010     gyro.start();
00011     }
00012 
00013 /*void gyro_process(){ // reading - 500 uS  // doesn't work in interrupt
00014     gyro.read(&gx,&gy,&gz,&ax,&ay,&az);
00015     
00016     //wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az);
00017     //wifi.printf("%.4f %.2f ",ay,gz);
00018 }*/
00019 
00020 void inertial_navigation(){
00021     gyro_v -= t_step*ax;  //inertial_navigation
00022     gyro_s += t_step*gyro_v;
00023     gyro_angle += t_step*gx; //gx?
00024     gyro_x += t_step*gyro_v*sin(gyro_angle);
00025     gyro_y += t_step*gyro_v*cos(gyro_angle); //sqr(1-x^2)
00026     //wifi.printf("v:%.1f, s:%.1f, deg:%.1f, x:%.1f, y:%.1f\n",gyro_v,gyro_s,gyro_angle,gyro_x,gyro_y);
00027 }
00028 
00029 void ax_filter(){
00030    static float ax_prev;
00031    ax = (ax + ax_prev)/2;
00032    ax_prev = ax;
00033     }