svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
gyro.h@22:14e85f2068c7, 2019-07-23 (annotated)
- Committer:
- dima285
- Date:
- Tue Jul 23 12:55:23 2019 +0000
- Branch:
- svoe
- Revision:
- 22:14e85f2068c7
- Parent:
- 19:2fe650d29823
23.07.2019
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dima285 | 19:2fe650d29823 | 1 | float gx,gy,gz,ax,ay,az; //deg/s, m/s^2 (x-vert, y-forward, z-left) |
Stas285 | 6:6e89cdc3db92 | 2 | #include "MPU6050.h" //Rewrite !!! |
Stas285 | 1:e2a6e523bf1f | 3 | |
Stas285 | 4:904b737ef08a | 4 | MPU6050 gyro(PB_9,PB_8); |
dima285 | 10:5bdd3dfd5f59 | 5 | |
Stas285 | 4:904b737ef08a | 6 | float gyro_x, gyro_y, gyro_angle, gyro_v, gyro_s; //cm, deg |
Stas285 | 4:904b737ef08a | 7 | |
Stas285 | 4:904b737ef08a | 8 | void gyro_init(){ |
Stas285 | 4:904b737ef08a | 9 | gyro.getID(); //dummy read (first transmission is bad) |
Stas285 | 4:904b737ef08a | 10 | gyro.start(); |
Stas285 | 4:904b737ef08a | 11 | } |
Stas285 | 1:e2a6e523bf1f | 12 | |
dima285 | 12:721a9ea55e91 | 13 | /*void gyro_process(){ // reading - 500 uS // doesn't work in interrupt |
Stas285 | 4:904b737ef08a | 14 | gyro.read(&gx,&gy,&gz,&ax,&ay,&az); |
dima285 | 9:8f98b1c277a4 | 15 | |
Stas285 | 6:6e89cdc3db92 | 16 | //wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az); |
dima285 | 9:8f98b1c277a4 | 17 | //wifi.printf("%.4f %.2f ",ay,gz); |
dima285 | 12:721a9ea55e91 | 18 | }*/ |
Stas285 | 1:e2a6e523bf1f | 19 | |
dima285 | 12:721a9ea55e91 | 20 | void inertial_navigation(){ |
dima285 | 12:721a9ea55e91 | 21 | gyro_v -= t_step*ax; //inertial_navigation |
dima285 | 12:721a9ea55e91 | 22 | gyro_s += t_step*gyro_v; |
dima285 | 12:721a9ea55e91 | 23 | gyro_angle += t_step*gx; //gx? |
dima285 | 12:721a9ea55e91 | 24 | gyro_x += t_step*gyro_v*sin(gyro_angle); |
dima285 | 12:721a9ea55e91 | 25 | gyro_y += t_step*gyro_v*cos(gyro_angle); //sqr(1-x^2) |
Stas285 | 6:6e89cdc3db92 | 26 | //wifi.printf("v:%.1f, s:%.1f, deg:%.1f, x:%.1f, y:%.1f\n",gyro_v,gyro_s,gyro_angle,gyro_x,gyro_y); |
Stas285 | 4:904b737ef08a | 27 | } |
Stas285 | 4:904b737ef08a | 28 | |
dima285 | 22:14e85f2068c7 | 29 | void ax_filter(){ |
dima285 | 22:14e85f2068c7 | 30 | static float ax_prev; |
dima285 | 22:14e85f2068c7 | 31 | ax = (ax + ax_prev)/2; |
dima285 | 22:14e85f2068c7 | 32 | ax_prev = ax; |
dima285 | 22:14e85f2068c7 | 33 | } |