gyro fusion
Dependencies: GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG
Diff: main.cpp
- Revision:
- 5:a3a1c87ad660
- Parent:
- 0:5432bdf904f9
--- 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