![](/media/cache/profiles/6ac13e46c8006acffa10354bf8724630_2.jpg.50x50_q85.jpg)
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
main.cpp
- Committer:
- grimwald
- Date:
- 2021-06-13
- Revision:
- 7:65ef53c3b7bc
- Parent:
- 6:81d8b03a9673
File content as of revision 7:65ef53c3b7bc:
#include "mbed.h" #include "GYRO_DISCO_L476VG.h" #include "COMPASS_DISCO_L476VG.h" // #define TE 0.01 GYRO_DISCO_L476VG gyro; COMPASS_DISCO_L476VG compass; 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; float GyroBuffer[3]; double gy_off=0; for(int i=0; i<NN; i++) { gyro.GetXYZ(GyroBuffer); gy_off=gy_off+GyroBuffer[1]/NN; } return(gy_off); } //fonction pour le calul angulaire double angle_zero(void) { const int NN=1000; int16_t AccBuffer[3]; double PI=4*atan(1.0); double ang_off=0; for(int i=0; i<NN; i++) { compass.AccGetXYZ(AccBuffer); double ang=(180/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]); ang_off=ang_off+ang/NN; } 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]; 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; //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; } } }