Les Vils Gredins / Mbed 2 deprecated DISCO_L476VG_AccGy2

cette fois faites import plutot que modifier direct bande de gredins

Dependencies:   GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG

main.cpp@9:187f56fa6db5, 2020-06-16 (annotated)

Committer:
Leonnn
Date:
Tue Jun 16 09:30:13 2020 +0000
Revision:
9:187f56fa6db5
Parent:
8:97589b322a4f
Child:
10:9a563a22a5e7
affichage coeffs

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"
Leonnn 8:97589b322a4f 3 #include "COMPASS_DISCO_L476VG.h"
Leonnn 8:97589b322a4f 4 #define PI 3.14159265358979323846
bcostm 0:5432bdf904f9 5
Leonnn 9:187f56fa6db5 6 double H0 = 1;// params filtrage
Leonnn 9:187f56fa6db5 7 double f0_lo = 5;
Leonnn 9:187f56fa6db5 8 double f0_hi = 1;
Leonnn 9:187f56fa6db5 9 double Te = 0.01;
Leonnn 9:187f56fa6db5 10 double coef[2];
Leonnn 9:187f56fa6db5 11 double xn[2];
Leonnn 9:187f56fa6db5 12 double yn[2];
Leonnn 9:187f56fa6db5 13
Leonnn 8:97589b322a4f 14 COMPASS_DISCO_L476VG compass;
bcostm 0:5432bdf904f9 15 GYRO_DISCO_L476VG gyro;
Leonnn 8:97589b322a4f 16
Leonnn 5:a50ff92004e0 17 Serial pc(SERIAL_TX, SERIAL_RX);
Leonnn 7:4d02d2486949 18 Ticker tick_mesure;
Leonnn 7:4d02d2486949 19
Leonnn 7:4d02d2486949 20 volatile bool flag_mesure = 0;
Leonnn 7:4d02d2486949 21
Leonnn 8:97589b322a4f 22 void mesure(void){flag_mesure = 1;}// ISR mesure
bcostm 0:5432bdf904f9 23
Leonnn 9:187f56fa6db5 24 void coef_filtre_PB(double H0,double f0,double Te,double* coef){
Leonnn 9:187f56fa6db5 25 coef[0] = (H0*Te)/(Te+2*(1/(2*PI*f0)));
Leonnn 9:187f56fa6db5 26 coef[1] = (Te-2*(1/(2*PI*f0)))/(Te+2*(1/(2*PI*f0)));
Leonnn 9:187f56fa6db5 27 }
Leonnn 9:187f56fa6db5 28
Leonnn 9:187f56fa6db5 29 void filtre_PB(double* xn,double* yn,double* coef){
Leonnn 9:187f56fa6db5 30 yn[0] = -coef[1]*yn[1] + coef[0]*(xn[0]+xn[1]); //yn = -b*yn-1 + a*(xn + xn-1)
Leonnn 9:187f56fa6db5 31 yn[1] = yn[0];
Leonnn 9:187f56fa6db5 32 xn[1] = xn[0];
Leonnn 9:187f56fa6db5 33 }
Leonnn 9:187f56fa6db5 34
bcostm 0:5432bdf904f9 35 int main()
bcostm 0:5432bdf904f9 36 {
Leonnn 7:4d02d2486949 37 float GyroBuffer[3], offset, GyrY;
Leonnn 7:4d02d2486949 38 offset = 0;
Leonnn 7:4d02d2486949 39 wait(0.5);
Leonnn 8:97589b322a4f 40 for(int i=0; i<100; i++){// séquence de mise à 0
Leonnn 7:4d02d2486949 41 gyro.GetXYZ(GyroBuffer);
Leonnn 7:4d02d2486949 42 offset = offset + GyroBuffer[1];
Leonnn 7:4d02d2486949 43 }
Leonnn 8:97589b322a4f 44 offset = offset/100;//-----------------fin mise à 0
Leonnn 7:4d02d2486949 45
Leonnn 7:4d02d2486949 46 tick_mesure.attach(&mesure, 0.01);
Leonnn 5:a50ff92004e0 47 pc.baud(115200);
Leonnn 8:97589b322a4f 48
Leonnn 8:97589b322a4f 49 unsigned int count = 0;
Leonnn 8:97589b322a4f 50 float Gyr_angle = 0;
Leonnn 8:97589b322a4f 51 float Acc_angle;
Leonnn 8:97589b322a4f 52 int16_t AccBuffer[3];
Leonnn 9:187f56fa6db5 53
Leonnn 9:187f56fa6db5 54
bcostm 0:5432bdf904f9 55
Leonnn 7:4d02d2486949 56 while(1) {
Leonnn 8:97589b322a4f 57 if(flag_mesure){// mesure Gyro
Leonnn 7:4d02d2486949 58 gyro.GetXYZ(GyroBuffer);
Leonnn 7:4d02d2486949 59 GyrY = GyroBuffer[1]-offset;
Leonnn 8:97589b322a4f 60 Gyr_angle = Gyr_angle + GyrY/100;
Leonnn 8:97589b322a4f 61 Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI;
Leonnn 7:4d02d2486949 62
Leonnn 7:4d02d2486949 63 if(count > 9){
Leonnn 8:97589b322a4f 64 compass.AccGetXYZ(AccBuffer);
Leonnn 9:187f56fa6db5 65 //pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000);
Leonnn 9:187f56fa6db5 66 coef_filtre_PB((double)1,(double)5,(double)0.01,coef);
Leonnn 9:187f56fa6db5 67 pc.printf("%f %f\n\r", coef[0], coef[1]);
Leonnn 7:4d02d2486949 68 count = 0;
Leonnn 7:4d02d2486949 69 }
Leonnn 7:4d02d2486949 70 else
Leonnn 7:4d02d2486949 71 count++;
Leonnn 7:4d02d2486949 72 flag_mesure = 0;
Leonnn 7:4d02d2486949 73 }
Leonnn 7:4d02d2486949 74 //wait(0.1);
bcostm 0:5432bdf904f9 75 }
bcostm 0:5432bdf904f9 76 }