cette fois faites import plutot que modifier direct bande de gredins
Dependencies: GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG
Diff: main.cpp
- Revision:
- 9:187f56fa6db5
- Parent:
- 8:97589b322a4f
- Child:
- 10:9a563a22a5e7
diff -r 97589b322a4f -r 187f56fa6db5 main.cpp --- a/main.cpp Fri Jun 12 13:48:55 2020 +0000 +++ b/main.cpp Tue Jun 16 09:30:13 2020 +0000 @@ -3,6 +3,14 @@ #include "COMPASS_DISCO_L476VG.h" #define PI 3.14159265358979323846 +double H0 = 1;// params filtrage +double f0_lo = 5; +double f0_hi = 1; +double Te = 0.01; +double coef[2]; +double xn[2]; +double yn[2]; + COMPASS_DISCO_L476VG compass; GYRO_DISCO_L476VG gyro; @@ -13,6 +21,17 @@ void mesure(void){flag_mesure = 1;}// ISR mesure +void coef_filtre_PB(double H0,double f0,double Te,double* coef){ + coef[0] = (H0*Te)/(Te+2*(1/(2*PI*f0))); + coef[1] = (Te-2*(1/(2*PI*f0)))/(Te+2*(1/(2*PI*f0))); +} + +void filtre_PB(double* xn,double* yn,double* coef){ + yn[0] = -coef[1]*yn[1] + coef[0]*(xn[0]+xn[1]); //yn = -b*yn-1 + a*(xn + xn-1) + yn[1] = yn[0]; + xn[1] = xn[0]; +} + int main() { float GyroBuffer[3], offset, GyrY; @@ -21,7 +40,6 @@ for(int i=0; i<100; i++){// séquence de mise à 0 gyro.GetXYZ(GyroBuffer); offset = offset + GyroBuffer[1]; - wait(0.01); } offset = offset/100;//-----------------fin mise à 0 @@ -32,6 +50,8 @@ float Gyr_angle = 0; float Acc_angle; int16_t AccBuffer[3]; + + while(1) { if(flag_mesure){// mesure Gyro @@ -42,7 +62,9 @@ if(count > 9){ compass.AccGetXYZ(AccBuffer); - pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000); + //pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000); + coef_filtre_PB((double)1,(double)5,(double)0.01,coef); + pc.printf("%f %f\n\r", coef[0], coef[1]); count = 0; } else