Vincent Soubirane / Mbed 2 deprecated Projet_Inclinometre

Inclinometre avec fusion de donnée

Dependencies:   mbed PinDetect

main.cpp@2:f63f0417ab5f, 2021-10-29 (annotated)

Committer:
natvich
Date:
Fri Oct 29 22:33:46 2021 +0000
Revision:
2:f63f0417ab5f
Parent:
0:e8167f37725c 
Projet d'inclinometre avec fusion de donnee

Who changed what in which revision?

UserRevisionLine numberNew contents of line
natvich 2:f63f0417ab5f 1 #include "mbed.h"
natvich 2:f63f0417ab5f 2 #include <math.h>
jmar7 0:e8167f37725c 3 #include "LSM9DS1.h"
jmar7 0:e8167f37725c 4
natvich 2:f63f0417ab5f 5
natvich 2:f63f0417ab5f 6
natvich 2:f63f0417ab5f 7 //
natvich 2:f63f0417ab5f 8 #define TE 0.01
natvich 2:f63f0417ab5f 9 LSM9DS1 lol(PB_9,PB_8, 0xD4, 0x38);
natvich 2:f63f0417ab5f 10
natvich 2:f63f0417ab5f 11
natvich 2:f63f0417ab5f 12
jmar7 0:e8167f37725c 13 Serial pc(USBTX, USBRX);
jmar7 0:e8167f37725c 14
natvich 2:f63f0417ab5f 15 float accx, accz, acccx, acccz, gy,ggy;
natvich 2:f63f0417ab5f 16
natvich 2:f63f0417ab5f 17
natvich 2:f63f0417ab5f 18 float test, test2;
natvich 2:f63f0417ab5f 19 Ticker calcul;
natvich 2:f63f0417ab5f 20 int i =0;
natvich 2:f63f0417ab5f 21
natvich 2:f63f0417ab5f 22 volatile bool flag=0;
natvich 2:f63f0417ab5f 23 double psig,psia;
natvich 2:f63f0417ab5f 24 double pacc_sortie[2];
natvich 2:f63f0417ab5f 25 double PI=4*atan(1.0);
natvich 2:f63f0417ab5f 26 double coef_a[2];
natvich 2:f63f0417ab5f 27 double coef_b[2];
natvich 2:f63f0417ab5f 28 double tau;
natvich 2:f63f0417ab5f 29 //les deux etat instant de l'accelerometre
natvich 2:f63f0417ab5f 30 double x_a[2];
natvich 2:f63f0417ab5f 31 double y_a[2];
natvich 2:f63f0417ab5f 32 //les deux etat instant de gyromètre
natvich 2:f63f0417ab5f 33 double x_b[2];
natvich 2:f63f0417ab5f 34 double y_b[2];
natvich 2:f63f0417ab5f 35
natvich 2:f63f0417ab5f 36 //affichage non filtré
natvich 2:f63f0417ab5f 37 double angle;
natvich 2:f63f0417ab5f 38 float addition ,moyenne;
natvich 2:f63f0417ab5f 39
natvich 2:f63f0417ab5f 40 //fonction de filtre passe bas
natvich 2:f63f0417ab5f 41 void filtre_PB(double* xn,double* yn,double* coef)
natvich 2:f63f0417ab5f 42 {
natvich 2:f63f0417ab5f 43 yn[0]=coef[0]*xn[0]+coef[0]*xn[1]-coef[1]*yn[1];
natvich 2:f63f0417ab5f 44 yn[1]=yn[0];
natvich 2:f63f0417ab5f 45 xn[1]=xn[0];
natvich 2:f63f0417ab5f 46
natvich 2:f63f0417ab5f 47 }
natvich 2:f63f0417ab5f 48 //calculer les coef de filtre passe bas
natvich 2:f63f0417ab5f 49 void coef_filtre_PB(double H0,double f0,double Te,double* coef)
natvich 2:f63f0417ab5f 50 {
natvich 2:f63f0417ab5f 51 coef[0]=(H0*Te)/(Te+(1/(PI*f0)));
natvich 2:f63f0417ab5f 52 coef[1]=(Te-(1/(PI*f0)))/(Te+1/(PI*f0));
natvich 2:f63f0417ab5f 53 }
natvich 2:f63f0417ab5f 54 //////////////////////////////////////////////////////////////////////////////////////////////////////////////supr offset
natvich 2:f63f0417ab5f 55 //fonctions pour calculer gy_off
natvich 2:f63f0417ab5f 56 double gyro_zero(void)
natvich 2:f63f0417ab5f 57 {
natvich 2:f63f0417ab5f 58 const int NN=10000;
natvich 2:f63f0417ab5f 59
natvich 2:f63f0417ab5f 60 double gy_off=0;
natvich 2:f63f0417ab5f 61 for(int i=0; i<NN; i++) {
natvich 2:f63f0417ab5f 62 lol.readGyro();
natvich 2:f63f0417ab5f 63 gy_off=gy_off+lol.gy;
natvich 2:f63f0417ab5f 64 // gy_off/10000; (fait après)
natvich 2:f63f0417ab5f 65 }
natvich 2:f63f0417ab5f 66 return(gy_off);
natvich 2:f63f0417ab5f 67 }
natvich 2:f63f0417ab5f 68 //fonction pour calculer ang_off
natvich 2:f63f0417ab5f 69 double angle_zero(void)
natvich 2:f63f0417ab5f 70 {
natvich 2:f63f0417ab5f 71 const int NN=1000;
natvich 2:f63f0417ab5f 72
natvich 2:f63f0417ab5f 73
natvich 2:f63f0417ab5f 74 double ang_off=0;
natvich 2:f63f0417ab5f 75 for(int i=0; i<NN; i++) {
natvich 2:f63f0417ab5f 76 lol.readAccel();
natvich 2:f63f0417ab5f 77 accx = lol.ax;
natvich 2:f63f0417ab5f 78 accz = lol.az;
natvich 2:f63f0417ab5f 79 double ang=(180/PI)*atan2(accx,accz);
natvich 2:f63f0417ab5f 80 ang_off=ang_off+ang;
natvich 2:f63f0417ab5f 81 ang_off/1000;
natvich 2:f63f0417ab5f 82 }
natvich 2:f63f0417ab5f 83 return ang_off;
natvich 2:f63f0417ab5f 84 }
natvich 2:f63f0417ab5f 85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
natvich 2:f63f0417ab5f 86 //fonction isr pour lever le flag
natvich 2:f63f0417ab5f 87 void mesure(void)
natvich 2:f63f0417ab5f 88 {
natvich 2:f63f0417ab5f 89 flag=1;
natvich 2:f63f0417ab5f 90 }
natvich 2:f63f0417ab5f 91
natvich 2:f63f0417ab5f 92 int main()
natvich 2:f63f0417ab5f 93 {
natvich 2:f63f0417ab5f 94
natvich 2:f63f0417ab5f 95
natvich 2:f63f0417ab5f 96
natvich 2:f63f0417ab5f 97
natvich 2:f63f0417ab5f 98
natvich 2:f63f0417ab5f 99 lol.begin();
jmar7 0:e8167f37725c 100 if (!lol.begin()) {
jmar7 0:e8167f37725c 101 pc.printf("Failed to communicate with LSM9DS1.\n");
natvich 2:f63f0417ab5f 102 }
natvich 2:f63f0417ab5f 103 lol.calibrate();
natvich 2:f63f0417ab5f 104
natvich 2:f63f0417ab5f 105 double gyr_off=gyro_zero();
natvich 2:f63f0417ab5f 106 gyr_off = gyr_off/10000;
natvich 2:f63f0417ab5f 107 double ang_off=angle_zero();
natvich 2:f63f0417ab5f 108 ang_off= ang_off/1000;
natvich 2:f63f0417ab5f 109 calcul.attach(&mesure,0.01);
natvich 2:f63f0417ab5f 110 unsigned char cpt=0;
natvich 2:f63f0417ab5f 111 coef_filtre_PB(1,0.1,0.01,coef_a);
natvich 2:f63f0417ab5f 112 tau=1/(2*PI*0.1);
natvich 2:f63f0417ab5f 113 coef_filtre_PB(tau,0.1,0.01,coef_b);
natvich 2:f63f0417ab5f 114 pc.baud(115200);
natvich 2:f63f0417ab5f 115
natvich 2:f63f0417ab5f 116
natvich 2:f63f0417ab5f 117
natvich 2:f63f0417ab5f 118 while(1) {
natvich 2:f63f0417ab5f 119 if(flag) {
natvich 2:f63f0417ab5f 120 lol.readAccel();
natvich 2:f63f0417ab5f 121 //calculer angle acceleration
natvich 2:f63f0417ab5f 122 acccx = lol.ax; //{
natvich 2:f63f0417ab5f 123 acccz = lol.az; //affichage de l'angle
natvich 2:f63f0417ab5f 124
natvich 2:f63f0417ab5f 125 psia=((180.0/PI)*atan2(acccx,acccz))-ang_off;
natvich 2:f63f0417ab5f 126 angle =((180.0/PI)*atan2(acccx,acccz))-ang_off;
natvich 2:f63f0417ab5f 127 //filtre d'etat instant presente de l'accelerometre doit egale angle mesure
natvich 2:f63f0417ab5f 128 x_a[0]=psia;
natvich 2:f63f0417ab5f 129 //on filtre le signal
natvich 2:f63f0417ab5f 130 filtre_PB(x_a,y_a,coef_a);
natvich 2:f63f0417ab5f 131 lol.readGyro();
natvich 2:f63f0417ab5f 132 //meme chose en gyrometre
natvich 2:f63f0417ab5f 133
natvich 2:f63f0417ab5f 134
natvich 2:f63f0417ab5f 135 x_b[0]=(lol.gy-gyr_off)*TE ;
natvich 2:f63f0417ab5f 136 filtre_PB(x_b,y_b,coef_b);
natvich 2:f63f0417ab5f 137
natvich 2:f63f0417ab5f 138 lol.readGyro();
natvich 2:f63f0417ab5f 139 addition = addition + lol.gy;
jmar7 0:e8167f37725c 140
natvich 2:f63f0417ab5f 141
natvich 2:f63f0417ab5f 142 if(cpt==9) {
natvich 2:f63f0417ab5f 143 cpt=0;
natvich 2:f63f0417ab5f 144 moyenne = moyenne +(addition/10.0)/1000;
natvich 2:f63f0417ab5f 145
natvich 2:f63f0417ab5f 146
natvich 2:f63f0417ab5f 147 //affichage en chaque cpt =9;
natvich 2:f63f0417ab5f 148
natvich 2:f63f0417ab5f 149 pc.printf("$%f %f %f;\n",y_a[0],y_b[0],y_a[0]+y_b[0]);
natvich 2:f63f0417ab5f 150
natvich 2:f63f0417ab5f 151 addition = 0;
natvich 2:f63f0417ab5f 152 }
natvich 2:f63f0417ab5f 153 cpt++;
natvich 2:f63f0417ab5f 154 flag=0;
natvich 2:f63f0417ab5f 155 }
jmar7 0:e8167f37725c 156 }
natvich 2:f63f0417ab5f 157
jmar7 0:e8167f37725c 158 }
natvich 2:f63f0417ab5f 159
natvich 2:f63f0417ab5f 160
natvich 2:f63f0417ab5f 161
natvich 2:f63f0417ab5f 162
natvich 2:f63f0417ab5f 163
natvich 2:f63f0417ab5f 164
natvich 2:f63f0417ab5f 165
natvich 2:f63f0417ab5f 166
natvich 2:f63f0417ab5f 167
natvich 2:f63f0417ab5f 168