E&R S3 prime / Mbed OS DISCO_AcGy3y

Final1x

Dependencies:   GYRO_DISCO_L476VG BSP_DISCO_L476VG COMPASS_DISCO_L476VG

main.cpp@6:6df9f66ca6bb, 2020-06-13 (annotated)

Committer:
gr91
Date:
Sat Jun 13 16:28:37 2020 +0000
Revision:
6:6df9f66ca6bb
Parent:
5:f4a35a2a9085
Child:
7:763b230d3b66
Final1

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