svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Diff: gyro.h
- Revision:
- 12:721a9ea55e91
- Parent:
- 10:5bdd3dfd5f59
- Child:
- 19:2fe650d29823
--- a/gyro.h Sun Oct 14 07:53:07 2018 +0000 +++ b/gyro.h Sat Nov 03 11:43:03 2018 +0000 @@ -7,26 +7,22 @@ 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 +/*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) - +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); } -void gyro_balance(){ //????? - //gyro_read_ay(); - } -