Vincent Soubirane
/
Projet_Inclinometre
Inclinometre avec fusion de donnée
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?
User | Revision | Line number | New 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 |