E&R S3 prime / Mbed OS DISCO_AcGy3y

Final1x

Dependencies:   GYRO_DISCO_L476VG BSP_DISCO_L476VG COMPASS_DISCO_L476VG

main.cpp@7:763b230d3b66, 2021-02-27 (annotated)

Committer:
gr91
Date:
Sat Feb 27 06:55:41 2021 +0000
Revision:
7:763b230d3b66
Parent:
6:6df9f66ca6bb
Child:
8:b0b8c9ed333e
xx

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gr91 7:763b230d3b66 1 // pour modifier la sensibilite du gyro
gr91 7:763b230d3b66 2 // dans le fichier stm32l476g_discovery_guroscope.c
gr91 7:763b230d3b66 3 // ligne 106
gr91 7:763b230d3b66 4 //L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_2000;
bcostm 0:5432bdf904f9 5 #include "mbed.h"
bcostm 0:5432bdf904f9 6 #include "GYRO_DISCO_L476VG.h"
gr91 6:6df9f66ca6bb 7 #include "COMPASS_DISCO_L476VG.h"
gr91 7:763b230d3b66 8 #define TE 0.01
gr91 7:763b230d3b66 9 #define FC 0.1
gr91 6:6df9f66ca6bb 10 //
bcostm 0:5432bdf904f9 11 GYRO_DISCO_L476VG gyro;
gr91 6:6df9f66ca6bb 12 COMPASS_DISCO_L476VG compass;
gr91 5:f4a35a2a9085 13 Serial pc(SERIAL_TX, SERIAL_RX,115200);
gr91 5:f4a35a2a9085 14 Ticker ticker;
bcostm 0:5432bdf904f9 15 DigitalOut led1(LED1);
gr91 5:f4a35a2a9085 16 volatile bool flag=0;
gr91 6:6df9f66ca6bb 17 float psig=0,psia;
gr91 6:6df9f66ca6bb 18 void coef_filtre_PB(double H0,double f0,double Te,double* coef)
gr91 6:6df9f66ca6bb 19 {
gr91 6:6df9f66ca6bb 20 double PI=4*atan(1.0);
gr91 6:6df9f66ca6bb 21 double tau=1/(2*PI*f0);
gr91 6:6df9f66ca6bb 22 coef[0]=H0/(1+(2*tau/Te));
gr91 6:6df9f66ca6bb 23 coef[1]=(Te-2*tau)/(Te+2*tau);
gr91 6:6df9f66ca6bb 24 }
gr91 6:6df9f66ca6bb 25 //
gr91 6:6df9f66ca6bb 26 void filtre_PB(double* xn,double* yn, double* coef)
gr91 6:6df9f66ca6bb 27 {
gr91 6:6df9f66ca6bb 28 yn[0]=coef[0]*(xn[0]+xn[1])-coef[1]*yn[1];
gr91 6:6df9f66ca6bb 29 xn[1]=xn[0];
gr91 6:6df9f66ca6bb 30 yn[1]=yn[0];
gr91 6:6df9f66ca6bb 31 }
gr91 6:6df9f66ca6bb 32 //
gr91 5:f4a35a2a9085 33 float gyro_zero(void)
gr91 5:f4a35a2a9085 34 {
gr91 5:f4a35a2a9085 35 const int NN=100000;
gr91 5:f4a35a2a9085 36 float GyroBuffer[3];
gr91 5:f4a35a2a9085 37 float gy_off=0;
gr91 6:6df9f66ca6bb 38 for(int i=0; i<NN; i++) {
gr91 5:f4a35a2a9085 39 gyro.GetXYZ(GyroBuffer);
gr91 5:f4a35a2a9085 40 gy_off=gy_off+GyroBuffer[1]/NN;
gr91 5:f4a35a2a9085 41 }
gr91 5:f4a35a2a9085 42 return(gy_off);
gr91 5:f4a35a2a9085 43 }
gr91 6:6df9f66ca6bb 44 //
gr91 6:6df9f66ca6bb 45 float angle_zero(void)
gr91 6:6df9f66ca6bb 46 {
gr91 6:6df9f66ca6bb 47 const int NN=100000;
gr91 6:6df9f66ca6bb 48 int16_t AccBuffer[3];
gr91 6:6df9f66ca6bb 49 double PI=4*atan(1.0);
gr91 6:6df9f66ca6bb 50 float ang_off=0;
gr91 6:6df9f66ca6bb 51 for(int i=0; i<NN; i++) {
gr91 6:6df9f66ca6bb 52 compass.AccGetXYZ(AccBuffer);
gr91 6:6df9f66ca6bb 53 double ang=(180/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]);
gr91 6:6df9f66ca6bb 54 ang_off=ang_off+ang/NN;
gr91 6:6df9f66ca6bb 55 }
gr91 6:6df9f66ca6bb 56 return ang_off;
gr91 6:6df9f66ca6bb 57 }
gr91 6:6df9f66ca6bb 58 //
gr91 5:f4a35a2a9085 59 void mesure(void)
gr91 5:f4a35a2a9085 60 {
gr91 5:f4a35a2a9085 61 flag=1;
gr91 5:f4a35a2a9085 62 }
gr91 6:6df9f66ca6bb 63 //
bcostm 0:5432bdf904f9 64 int main()
bcostm 0:5432bdf904f9 65 {
bcostm 0:5432bdf904f9 66 float GyroBuffer[3];
gr91 6:6df9f66ca6bb 67 int16_t AccBuffer[3];
gr91 6:6df9f66ca6bb 68 double coef_acc[2];
gr91 6:6df9f66ca6bb 69 double coef_gyr[2];
gr91 6:6df9f66ca6bb 70 double Acc[2];
gr91 6:6df9f66ca6bb 71 double AccF[2];
gr91 6:6df9f66ca6bb 72 double Gyr[2];
gr91 6:6df9f66ca6bb 73 double GyrF[2];
gr91 6:6df9f66ca6bb 74 wait(1);
gr91 6:6df9f66ca6bb 75 pc.printf("Super Inclinometre \n");
gr91 7:763b230d3b66 76 coef_filtre_PB(1,FC,TE,coef_acc); // H0 f0 Te
gr91 7:763b230d3b66 77 coef_filtre_PB(1/(2*3.1415926*FC),FC,TE,coef_gyr); // H0 f0 Te
gr91 6:6df9f66ca6bb 78 //pc.printf("a= %f b=%f \n\r",coef_acc[0],coef_acc[1]);
gr91 6:6df9f66ca6bb 79 float gyoff=gyro_zero();
gr91 6:6df9f66ca6bb 80 float anoff=angle_zero();
gr91 7:763b230d3b66 81 ticker.attach(&mesure,TE);
gr91 5:f4a35a2a9085 82 unsigned char cpt=0;
gr91 6:6df9f66ca6bb 83 //
bcostm 0:5432bdf904f9 84 while(1) {
gr91 5:f4a35a2a9085 85 if(flag) {
gr91 6:6df9f66ca6bb 86 compass.AccGetXYZ(AccBuffer);
gr91 6:6df9f66ca6bb 87 psia=(180.0/3.1415926)*atan2((float)AccBuffer[0],(float)AccBuffer[2])-anoff;
gr91 5:f4a35a2a9085 88 gyro.GetXYZ(GyroBuffer);
gr91 5:f4a35a2a9085 89 psig=psig+(GyroBuffer[1]-gyoff)*0.01/1000;
gr91 6:6df9f66ca6bb 90 Acc[0]=psia;
gr91 6:6df9f66ca6bb 91 Gyr[0]=(GyroBuffer[1]-gyoff)/1000;
gr91 6:6df9f66ca6bb 92 filtre_PB(Acc,AccF,coef_acc);
gr91 6:6df9f66ca6bb 93 filtre_PB(Gyr,GyrF,coef_gyr);
gr91 6:6df9f66ca6bb 94 double fusion=AccF[0]+GyrF[0];
gr91 5:f4a35a2a9085 95 if(cpt==9) {
gr91 5:f4a35a2a9085 96 cpt=0;
gr91 5:f4a35a2a9085 97 led1 = !led1;
gr91 6:6df9f66ca6bb 98 // angle accelero, angne gyro, angle acc. filtré, gyro filtré, angle fusion
gr91 6:6df9f66ca6bb 99 //pc.printf("$%f %f %f %f %f;\n",psia,psig,AccF[0],GyrF[0],fusion);
gr91 6:6df9f66ca6bb 100 pc.printf("$%f %f %f;\n",AccF[0],GyrF[0],fusion);
gr91 6:6df9f66ca6bb 101
gr91 5:f4a35a2a9085 102 }
gr91 5:f4a35a2a9085 103 cpt++;
gr91 5:f4a35a2a9085 104 flag=0;
gr91 5:f4a35a2a9085 105 }
bcostm 0:5432bdf904f9 106 }
bcostm 0:5432bdf904f9 107 }