Vincent Soubirane
/
Projet_Inclinometre
Inclinometre avec fusion de donnée
Revision 2:f63f0417ab5f, committed 2021-10-29
- Comitter:
- natvich
- Date:
- Fri Oct 29 22:33:46 2021 +0000
- Parent:
- 1:87d535bf8c53
- Commit message:
- Projet d'inclinometre avec fusion de donnee
Changed in this revision
LSM9DS1.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/LSM9DS1.h Mon Oct 26 16:14:04 2015 +0000 +++ b/LSM9DS1.h Fri Oct 29 22:33:46 2021 +0000 @@ -312,7 +312,7 @@ * Any OR'd combination of ZIEN, YIEN, XIEN * - activeLow = Interrupt active configuration * Can be either INT_ACTIVE_HIGH or INT_ACTIVE_LOW - */ - latch: latch gyroscope interrupt request. + - latch: latch gyroscope interrupt request. */ void configMagInt(uint8_t generator, h_lactive activeLow, bool latch = true); /** configMagThs() -- Configure the threshold of a gyroscope axis
--- a/main.cpp Mon Oct 26 16:14:04 2015 +0000 +++ b/main.cpp Fri Oct 29 22:33:46 2021 +0000 @@ -1,29 +1,168 @@ +#include "mbed.h" +#include <math.h> #include "LSM9DS1.h" -DigitalOut myled(LED1); + + +// +#define TE 0.01 +LSM9DS1 lol(PB_9,PB_8, 0xD4, 0x38); + + + Serial pc(USBTX, USBRX); -int main() { - //LSM9DS1 lol(p9, p10, 0x6B, 0x1E); - LSM9DS1 lol(p9, p10, 0xD6, 0x3C); - lol.begin(); +float accx, accz, acccx, acccz, gy,ggy; + + +float test, test2; +Ticker calcul; +int i =0; + +volatile bool flag=0; +double psig,psia; +double pacc_sortie[2]; +double PI=4*atan(1.0); +double coef_a[2]; +double coef_b[2]; +double tau; +//les deux etat instant de l'accelerometre +double x_a[2]; +double y_a[2]; +//les deux etat instant de gyromètre +double x_b[2]; +double y_b[2]; + +//affichage non filtré +double angle; +float addition ,moyenne; + +//fonction de filtre passe bas +void filtre_PB(double* xn,double* yn,double* coef) +{ + yn[0]=coef[0]*xn[0]+coef[0]*xn[1]-coef[1]*yn[1]; + yn[1]=yn[0]; + xn[1]=xn[0]; + +} +//calculer les coef de filtre passe bas +void coef_filtre_PB(double H0,double f0,double Te,double* coef) +{ + coef[0]=(H0*Te)/(Te+(1/(PI*f0))); + coef[1]=(Te-(1/(PI*f0)))/(Te+1/(PI*f0)); +} +//////////////////////////////////////////////////////////////////////////////////////////////////////////////supr offset +//fonctions pour calculer gy_off +double gyro_zero(void) +{ + const int NN=10000; + + double gy_off=0; + for(int i=0; i<NN; i++) { + lol.readGyro(); + gy_off=gy_off+lol.gy; + // gy_off/10000; (fait après) + } + return(gy_off); +} +//fonction pour calculer ang_off +double angle_zero(void) +{ + const int NN=1000; + + + double ang_off=0; + for(int i=0; i<NN; i++) { + lol.readAccel(); + accx = lol.ax; + accz = lol.az; + double ang=(180/PI)*atan2(accx,accz); + ang_off=ang_off+ang; + ang_off/1000; + } + return ang_off; +} +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//fonction isr pour lever le flag +void mesure(void) +{ + flag=1; +} + +int main() +{ + + + + + + lol.begin(); if (!lol.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); - } - lol.calibrate(); - while(1) { - lol.readTemp(); - lol.readMag(); - lol.readGyro(); + } + lol.calibrate(); + + double gyr_off=gyro_zero(); + gyr_off = gyr_off/10000; + double ang_off=angle_zero(); + ang_off= ang_off/1000; + calcul.attach(&mesure,0.01); + unsigned char cpt=0; + coef_filtre_PB(1,0.1,0.01,coef_a); + tau=1/(2*PI*0.1); + coef_filtre_PB(tau,0.1,0.01,coef_b); + pc.baud(115200); + + + +while(1) { + if(flag) { + lol.readAccel(); + //calculer angle acceleration + acccx = lol.ax; //{ + acccz = lol.az; //affichage de l'angle + + psia=((180.0/PI)*atan2(acccx,acccz))-ang_off; + angle =((180.0/PI)*atan2(acccx,acccz))-ang_off; + //filtre d'etat instant presente de l'accelerometre doit egale angle mesure + x_a[0]=psia; + //on filtre le signal + filtre_PB(x_a,y_a,coef_a); + lol.readGyro(); + //meme chose en gyrometre + + + x_b[0]=(lol.gy-gyr_off)*TE ; + filtre_PB(x_b,y_b,coef_b); + + lol.readGyro(); + addition = addition + lol.gy; - //pc.printf("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz); - //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz)); - pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz); - pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az); - pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz); - myled = 1; - wait(2); - myled = 0; - wait(2); + + if(cpt==9) { + cpt=0; + moyenne = moyenne +(addition/10.0)/1000; + + + //affichage en chaque cpt =9; + + pc.printf("$%f %f %f;\n",y_a[0],y_b[0],y_a[0]+y_b[0]); + + addition = 0; + } + cpt++; + flag=0; + } } + } + + + + + + + + + + \ No newline at end of file