![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
gyro fusion
Dependencies: GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG
Revision 5:a3a1c87ad660, committed 2021-06-10
- Comitter:
- iinewbieii
- Date:
- Thu Jun 10 12:34:54 2021 +0000
- Parent:
- 4:25b2a1150cd0
- Commit message:
- gzidaogh
Changed in this revision
diff -r 25b2a1150cd0 -r a3a1c87ad660 COMPASS_DISCO_L476VG.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/COMPASS_DISCO_L476VG.lib Thu Jun 10 12:34:54 2021 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/COMPASS_DISCO_L476VG/#6508fa5521e0
diff -r 25b2a1150cd0 -r a3a1c87ad660 main.cpp --- a/main.cpp Wed Nov 20 15:13:34 2019 +0100 +++ b/main.cpp Thu Jun 10 12:34:54 2021 +0000 @@ -1,25 +1,98 @@ #include "mbed.h" #include "GYRO_DISCO_L476VG.h" +#include "COMPASS_DISCO_L476VG.h" -GYRO_DISCO_L476VG gyro; +#define Te 0.01 +#define r2d 57.2957795131 +#define PI 3.14159265359 + +GYRO_DISCO_L476VG gyro; +COMPASS_DISCO_L476VG compass; + +Serial pc(USBTX, USBRX); +Ticker t1; + +volatile bool flag = 0 ; + +double coef_a[2]; +double coef_g[2]; -DigitalOut led1(LED1); +double xn[2]; +double yn[2]; + +double sortie_a[2]; +double sortie_g[2]; + +double angle_y_acc[2]; +double angle_y_gyro[2]; + +float x,z; +float derive; +float offset; + +void coef_filtre(double,double ,double*); +void filtre_PB(double *, double *, double *); + +void mesure(); +int u; + int main() { float GyroBuffer[3]; - - printf("Gyroscope started\n"); - - while(1) { - // Read Gyroscope values + int16_t AccBuffer[3]; + pc.baud(115200); + t1.attach(&mesure,Te); + + coef_filtre(1.00,0.1,coef_a); + coef_filtre(1/(2*PI*0.1),0.1,coef_g); + + for(int a = 0;a<=10000;a++){ gyro.GetXYZ(GyroBuffer); - // Display values - printf("X = %f\n", GyroBuffer[0]); - printf("Y = %f\n", GyroBuffer[1]); - printf("Z = %f\n", GyroBuffer[2]); - printf("\033[3A"); // Moves cursor up x lines (x value is between [ and A) - led1 = !led1; - wait(1); + derive += GyroBuffer[1]/10000; + } + for(int a = 0;a<=1000;a++){ + compass.AccGetXYZ(AccBuffer); + x = AccBuffer[0]*0.061/1000; + z = AccBuffer[2]*0.061/1000; + offset += ((atan2((double)x,(double)z))*r2d)/1000; + } + + while(1) { + + if(flag) { + flag = 0; + compass.AccGetXYZ(AccBuffer); + gyro.GetXYZ(GyroBuffer); + x = AccBuffer[0]*0.061/1000; + z = AccBuffer[2]*0.061/1000; + angle_y_gyro[0] = (GyroBuffer[1]-derive)/1000; + angle_y_acc[0] = (atan2((double)x,(double)z))*r2d-offset; + + filtre_PB(angle_y_acc,sortie_a,coef_a); + filtre_PB(angle_y_gyro,sortie_g,coef_g); + + if((u%10)==0) { + pc.printf("$%lf %lf %lf;\n",sortie_g[0],sortie_a[0],sortie_a[0]+sortie_g[0]); + } + } } } + +void mesure() +{ + flag = 1; + u++; +} + +void coef_filtre(double H0,double F0,double *coef) +{ + coef[0] = (H0*Te)/( Te + (1/(PI*F0))); + coef[1] = (Te -(1/(PI*F0)))/( Te + (1/(PI*F0))); +} +void filtre_PB(double *xn, double *yn, double *coef) +{ + yn[0] = coef[0]*(xn[1]+xn[0])-coef[1]*yn[1]; + yn[1] = yn[0]; + xn[1] = xn[0]; +} \ No newline at end of file
diff -r 25b2a1150cd0 -r a3a1c87ad660 mbed-os.lib --- a/mbed-os.lib Wed Nov 20 15:13:34 2019 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://github.com/ARMmbed/mbed-os/#b81aeff1a3e171c6421984faa2cc18d0e35746c0
diff -r 25b2a1150cd0 -r a3a1c87ad660 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jun 10 12:34:54 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file