Inclinomètre a fusion de données entre gyromètre et accéléromètre
Dependencies: GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG
Revision 7:65ef53c3b7bc, committed 2021-06-13
- Comitter:
- grimwald
- Date:
- Sun Jun 13 10:18:17 2021 +0000
- Parent:
- 6:81d8b03a9673
- Commit message:
- Fusion accel et gyro;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 81d8b03a9673 -r 65ef53c3b7bc main.cpp --- a/main.cpp Mon Jun 15 14:48:08 2020 +0000 +++ b/main.cpp Sun Jun 13 10:18:17 2021 +0000 @@ -8,8 +8,33 @@ Serial pc(SERIAL_TX, SERIAL_RX,115200); Ticker ticker; DigitalOut led1(LED1); +//variables volatile bool flag=0; double psig=0,psia; +double pacc_sortie[2]; +double PI=4*atan(1.0); +double coef_a[2]; +double coef_b[2]; +double tau; +double x_a[2]; +double y_a[2]; +double x_b[2]; +double y_b[2]; +//Fonction qui va permettre le filtrage +void filtre_PB(double* xn,double* yn,double* coef) +{ + yn[0]=coef[0]*xn[0]+coef[0]*xn[1]-coef[1]*yn[1]; + yn[1]=yn[0]; + xn[1]=xn[0]; + +} +//fonction pour calculer les coefficient a et b de la fonction de transfert +void coef_filtre_PB(double H0,double f0,double Te,double* coef) +{ + coef[0]=(H0*Te)/(Te+(1/PI/f0)); + coef[1]=(Te-(1/PI/f0))/(Te+1/PI/f0); +} +//fonction pour le gyroscope double gyro_zero(void) { const int NN=10000; @@ -21,6 +46,7 @@ } return(gy_off); } +//fonction pour le calul angulaire double angle_zero(void) { const int NN=1000; @@ -34,34 +60,47 @@ } return ang_off; } +//fonction qui leve un flag qui va etre utiliser dans le main void mesure(void) { flag=1; } int main() { + float GyroBuffer[3]; int16_t AccBuffer[3]; - printf("Super inclinometre\n\r"); + double PI=4*atan(1.0); double gyr_off=gyro_zero(); double ang_off=angle_zero(); + //prise de mesure toute les 10 ms ticker.attach(&mesure,TE); unsigned char cpt=0; + //definition des coef a et b + coef_filtre_PB(1,0.1,0.01,coef_a); + tau=1/(2*PI*0.1); + coef_filtre_PB(tau,0.1,0.01,coef_b); while(1) { + //calcul des angles pour l'accelerometre if(flag) { compass.AccGetXYZ(AccBuffer); psia=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off; + x_a[0]=psia; + filtre_PB(x_a,y_a,coef_a); gyro.GetXYZ(GyroBuffer); psig=psig+(GyroBuffer[1]-gyr_off)*TE/1000; + x_b[0]=(GyroBuffer[1]-gyr_off)/1000; + filtre_PB(x_b,y_b,coef_b); + if(cpt==9) { cpt=0; - led1 = !led1; - pc.printf("$%f %f;\n",psia,psig); + //affichage au bout de 10 mesure sur serial port plotter + pc.printf("$%f %f %f;\n",y_a[0],y_b[0],y_a[0]+y_b[0]); } cpt++; flag=0; } } -} +} \ No newline at end of file