houmane fathi / Mbed 2 deprecated DISCO_AcGy1final

Code rendu

Dependencies:   GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG

main.cpp@7:24a3e72617fa, 2021-06-14 (annotated)

Committer:
houms
Date:
Mon Jun 14 21:52:39 2021 +0000
Revision:
7:24a3e72617fa
Parent:
6:81d8b03a9673 
Programme inclinometre;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:5432bdf904f9 1 #include "mbed.h"
bcostm 0:5432bdf904f9 2 #include "GYRO_DISCO_L476VG.h"
gr91 6:81d8b03a9673 3 #include "COMPASS_DISCO_L476VG.h"
gr91 6:81d8b03a9673 4 //
gr91 6:81d8b03a9673 5 #define TE 0.01
houms 7:24a3e72617fa 6 #define F0 0.1
houms 7:24a3e72617fa 7 #define H 1
houms 7:24a3e72617fa 8
bcostm 0:5432bdf904f9 9 GYRO_DISCO_L476VG gyro;
gr91 6:81d8b03a9673 10 COMPASS_DISCO_L476VG compass;
gr91 5:f4a35a2a9085 11 Serial pc(SERIAL_TX, SERIAL_RX,115200);
gr91 5:f4a35a2a9085 12 Ticker ticker;
bcostm 0:5432bdf904f9 13 DigitalOut led1(LED1);
gr91 5:f4a35a2a9085 14 volatile bool flag=0;
houms 7:24a3e72617fa 15
houms 7:24a3e72617fa 16 double psiafiltre[2];
houms 7:24a3e72617fa 17 double psia[2]={0};
houms 7:24a3e72617fa 18 double coef[2];
houms 7:24a3e72617fa 19
houms 7:24a3e72617fa 20 double psig[2]={0};
houms 7:24a3e72617fa 21 double psigfiltre[2];
houms 7:24a3e72617fa 22 double coef2[2];
houms 7:24a3e72617fa 23
houms 7:24a3e72617fa 24 double inclonometre ;
houms 7:24a3e72617fa 25 double T;
houms 7:24a3e72617fa 26
gr91 6:81d8b03a9673 27 double gyro_zero(void)
gr91 5:f4a35a2a9085 28 {
gr91 6:81d8b03a9673 29 const int NN=10000;
gr91 5:f4a35a2a9085 30 float GyroBuffer[3];
gr91 6:81d8b03a9673 31 double gy_off=0;
gr91 6:81d8b03a9673 32 for(int i=0; i<NN; i++) {
gr91 5:f4a35a2a9085 33 gyro.GetXYZ(GyroBuffer);
gr91 5:f4a35a2a9085 34 gy_off=gy_off+GyroBuffer[1]/NN;
gr91 5:f4a35a2a9085 35 }
gr91 5:f4a35a2a9085 36 return(gy_off);
gr91 5:f4a35a2a9085 37 }
gr91 6:81d8b03a9673 38 double angle_zero(void)
gr91 6:81d8b03a9673 39 {
gr91 6:81d8b03a9673 40 const int NN=1000;
gr91 6:81d8b03a9673 41 int16_t AccBuffer[3];
gr91 6:81d8b03a9673 42 double PI=4*atan(1.0);
gr91 6:81d8b03a9673 43 double ang_off=0;
gr91 6:81d8b03a9673 44 for(int i=0; i<NN; i++) {
gr91 6:81d8b03a9673 45 compass.AccGetXYZ(AccBuffer);
gr91 6:81d8b03a9673 46 double ang=(180/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]);
houms 7:24a3e72617fa 47 ang_off=ang_off
houms 7:24a3e72617fa 48 +ang/NN;
gr91 6:81d8b03a9673 49 }
gr91 6:81d8b03a9673 50 return ang_off;
gr91 6:81d8b03a9673 51 }
houms 7:24a3e72617fa 52 void filtre_PB(double* xn,double* yn,double* coef1)
houms 7:24a3e72617fa 53 {
houms 7:24a3e72617fa 54 yn[0] = (xn[0]+xn[1])*coef1[0]- yn[1]*coef1[1];
houms 7:24a3e72617fa 55 yn[1]= yn[0];
houms 7:24a3e72617fa 56 xn[1] = xn[0];
houms 7:24a3e72617fa 57
houms 7:24a3e72617fa 58 }
houms 7:24a3e72617fa 59
houms 7:24a3e72617fa 60 void coef_filtre_PB(double H10,double f10,double TeE,double* cof)
houms 7:24a3e72617fa 61 {
houms 7:24a3e72617fa 62
houms 7:24a3e72617fa 63 float T2 ;
houms 7:24a3e72617fa 64 T2=1.00/(3.14*f10);
houms 7:24a3e72617fa 65 cof[0]=TeE*H10/(TeE+T2) ;
houms 7:24a3e72617fa 66 cof[1]=(TeE-T2)/(TeE+T2);
houms 7:24a3e72617fa 67 }
houms 7:24a3e72617fa 68
gr91 5:f4a35a2a9085 69 void mesure(void)
gr91 5:f4a35a2a9085 70 {
gr91 5:f4a35a2a9085 71 flag=1;
gr91 5:f4a35a2a9085 72 }
bcostm 0:5432bdf904f9 73 int main()
bcostm 0:5432bdf904f9 74 {
bcostm 0:5432bdf904f9 75 float GyroBuffer[3];
gr91 6:81d8b03a9673 76 int16_t AccBuffer[3];
gr91 6:81d8b03a9673 77 printf("Super inclinometre\n\r");
gr91 6:81d8b03a9673 78 double PI=4*atan(1.0);
gr91 6:81d8b03a9673 79 double gyr_off=gyro_zero();
gr91 6:81d8b03a9673 80 double ang_off=angle_zero();
gr91 6:81d8b03a9673 81 ticker.attach(&mesure,TE);
houms 7:24a3e72617fa 82 coef_filtre_PB(H,F0,TE,coef);
houms 7:24a3e72617fa 83 T=(1.00/(2*3.14*F0));
houms 7:24a3e72617fa 84 coef_filtre_PB(T,F0,TE,coef2);
houms 7:24a3e72617fa 85
gr91 5:f4a35a2a9085 86 unsigned char cpt=0;
houms 7:24a3e72617fa 87
bcostm 0:5432bdf904f9 88 while(1) {
gr91 5:f4a35a2a9085 89 if(flag) {
gr91 6:81d8b03a9673 90 compass.AccGetXYZ(AccBuffer);
houms 7:24a3e72617fa 91 psia[0]=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off;
houms 7:24a3e72617fa 92 filtre_PB(psia,psiafiltre,coef);
gr91 5:f4a35a2a9085 93 gyro.GetXYZ(GyroBuffer);
houms 7:24a3e72617fa 94 psig[0]=(GyroBuffer[1]-gyr_off)/1000;
houms 7:24a3e72617fa 95 filtre_PB(psig,psigfiltre,coef2);
houms 7:24a3e72617fa 96
gr91 5:f4a35a2a9085 97 if(cpt==9) {
gr91 5:f4a35a2a9085 98 cpt=0;
houms 7:24a3e72617fa 99
gr91 5:f4a35a2a9085 100 led1 = !led1;
houms 7:24a3e72617fa 101 inclonometre= psiafiltre[0]+psigfiltre[0];
houms 7:24a3e72617fa 102 pc.printf("$%f %f %f;\n",psiafiltre[0],psigfiltre[0],inclonometre);
gr91 5:f4a35a2a9085 103 }
gr91 5:f4a35a2a9085 104 cpt++;
gr91 5:f4a35a2a9085 105 flag=0;
gr91 5:f4a35a2a9085 106 }
bcostm 0:5432bdf904f9 107 }
bcostm 0:5432bdf904f9 108 }