![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
cette fois faites import plutot que modifier direct bande de gredins
Dependencies: GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG
Revision 10:9a563a22a5e7, committed 2020-06-16
- Comitter:
- Leonnn
- Date:
- Tue Jun 16 11:23:59 2020 +0000
- Parent:
- 9:187f56fa6db5
- Commit message:
- lissage acc
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Jun 16 09:30:13 2020 +0000 +++ b/main.cpp Tue Jun 16 11:23:59 2020 +0000 @@ -3,10 +3,6 @@ #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]; @@ -19,7 +15,7 @@ volatile bool flag_mesure = 0; -void mesure(void){flag_mesure = 1;}// ISR mesure +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))); @@ -27,21 +23,21 @@ } 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[0] = -coef[1]*yn[1] + coef[0]*(xn[0]+xn[1]); yn[1] = yn[0]; xn[1] = xn[0]; } -int main() +int main()/////////////////////////////////////////////////////////////////////////////////////////// { - float GyroBuffer[3], offset, GyrY; - offset = 0; + float GyroBuffer[3], offsetGyr, GyrY; + offsetGyr = 0; wait(0.5); - for(int i=0; i<100; i++){// séquence de mise à 0 + for(int i=0; i<100; i++){// --------------------------------------------séquence de mise à 0 gyro.GetXYZ(GyroBuffer); - offset = offset + GyroBuffer[1]; + offsetGyr = offsetGyr + GyroBuffer[1]; } - offset = offset/100;//-----------------fin mise à 0 + offsetGyr = offsetGyr/100;//------------------------------------------------------fin mise à 0 tick_mesure.attach(&mesure, 0.01); pc.baud(115200); @@ -51,20 +47,21 @@ float Acc_angle; int16_t AccBuffer[3]; + coef_filtre_PB((double)1,(double)0.5,(double)0.01,coef);//------------------calcul coeff acc - - while(1) { + while(1) { ////////////////////////////////////////////////////////////////////////////////////// if(flag_mesure){// mesure Gyro - gyro.GetXYZ(GyroBuffer); - GyrY = GyroBuffer[1]-offset; - Gyr_angle = Gyr_angle + GyrY/100; - Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI; + gyro.GetXYZ(GyroBuffer);//------------------------------------------get gyro + GyrY = GyroBuffer[1]-offsetGyr;//--------------------------------------compensation gyro + Gyr_angle = Gyr_angle + GyrY/100000;//------------------------------clacul gyro (intégration) - if(count > 9){ - compass.AccGetXYZ(AccBuffer); - //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]); + compass.AccGetXYZ(AccBuffer); //------------------------------------get acc + Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI-3.32;//calcul acc + compensation + xn[0] = Acc_angle;//------------------------------------------------début filtrage acc + filtre_PB(xn,yn,coef); + Acc_angle = yn[0];//------------------------------------------------fin filtrage acc + if(count > 9){ + pc.printf("$%f %f;", Acc_angle, Gyr_angle); count = 0; } else