svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
gyro.h
- Committer:
- dima285
- Date:
- 2019-02-24
- Revision:
- 19:2fe650d29823
- Parent:
- 12:721a9ea55e91
- Child:
- 22:14e85f2068c7
File content as of revision 19:2fe650d29823:
float gx,gy,gz,ax,ay,az; //deg/s, m/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) 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); }*/ void inertial_navigation(){ gyro_v -= t_step*ax; //inertial_navigation gyro_s += t_step*gyro_v; gyro_angle += t_step*gx; //gx? gyro_x += t_step*gyro_v*sin(gyro_angle); gyro_y += t_step*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); }