Diff: gyro.h

Revision:

12:721a9ea55e91

Parent:

10:5bdd3dfd5f59

Child:

19:2fe650d29823

diff -r f7934de1d03a -r 721a9ea55e91 gyro.h
--- 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();
-    }
-