Johan Beverini / Mbed 2 deprecated 9DOF-Stick

code projet M1 + ensil

Dependencies:   ADXL345 HMC5843 ITG3200 mbed

Fork of 9DOF-Stick by Uwe Gartmann

main.cpp@3:15f5e8de8533, 2018-03-28 (annotated)

Committer:
JohanBeverini
Date:
Wed Mar 28 17:03:44 2018 +0000
Revision:
3:15f5e8de8533
Parent:
2:c340f1bda440 
code projet M1 + ensil

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Digixx 0:9410b3e6aea5 1 #include "mbed.h"
Digixx 0:9410b3e6aea5 2 #include "HMC5843.h"
Digixx 0:9410b3e6aea5 3 #include "ADXL345.h"
Digixx 0:9410b3e6aea5 4 #include "ITG3200.h"
JohanBeverini 2:c340f1bda440 5 #include <math.h>
JohanBeverini 2:c340f1bda440 6
JohanBeverini 2:c340f1bda440 7 #define PI 3.14159265358979323846f
Digixx 0:9410b3e6aea5 8
Digixx 0:9410b3e6aea5 9 DigitalOut myled(LED1);
Digixx 0:9410b3e6aea5 10
JohanBeverini 2:c340f1bda440 11 HMC5843 cmp(D4, D5); // sda, scl
JohanBeverini 2:c340f1bda440 12 ADXL345 acc(D4, D5); // sda, scl
JohanBeverini 2:c340f1bda440 13 ITG3200 gyr(D4, D5); // sda, scl
JohanBeverini 2:c340f1bda440 14
JohanBeverini 2:c340f1bda440 15 Serial pc(SERIAL_TX, SERIAL_RX); // tx, rx
JohanBeverini 2:c340f1bda440 16 Serial BT(PA_9, PA_10); // tx, rx
JohanBeverini 2:c340f1bda440 17
JohanBeverini 2:c340f1bda440 18 Timer t;
JohanBeverini 2:c340f1bda440 19
JohanBeverini 2:c340f1bda440 20 float acc_x,acc_y,acc_z;
JohanBeverini 2:c340f1bda440 21 float mag_x,mag_y,mag_z;
JohanBeverini 2:c340f1bda440 22 float gyr_x,gyr_y,gyr_z;
JohanBeverini 2:c340f1bda440 23
JohanBeverini 2:c340f1bda440 24 float theta,phi,psi;
JohanBeverini 2:c340f1bda440 25 float theta1,phi1,psi1;
JohanBeverini 2:c340f1bda440 26
JohanBeverini 2:c340f1bda440 27 double Last, Now;
JohanBeverini 2:c340f1bda440 28 bool START = true;
JohanBeverini 2:c340f1bda440 29 int count;
JohanBeverini 2:c340f1bda440 30
JohanBeverini 2:c340f1bda440 31 float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0};
JohanBeverini 2:c340f1bda440 32 float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0};
JohanBeverini 2:c340f1bda440 33 float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0};
JohanBeverini 2:c340f1bda440 34 float a_coef[3]={1.0000, -0.3695, 0.1958}; //coefs pour filtre PB IIR2 à fréquence coupure normalisé 0.4
JohanBeverini 2:c340f1bda440 35 float b_coef[3]={0.2066, 0.4131, 0.2066};
JohanBeverini 2:c340f1bda440 36 float theta_filtre, phi_filtre, psi_filtre;
JohanBeverini 2:c340f1bda440 37
JohanBeverini 2:c340f1bda440 38 float matrice[3][3], resultat[3];
JohanBeverini 2:c340f1bda440 39 float ca,cb,cc,sa,sb,sc;
JohanBeverini 2:c340f1bda440 40 float tableau_x[10], tableau_y[10], tableau_z[10];
JohanBeverini 2:c340f1bda440 41
Digixx 0:9410b3e6aea5 42
Digixx 0:9410b3e6aea5 43 int main() {
JohanBeverini 2:c340f1bda440 44 pc.baud(9600);
JohanBeverini 2:c340f1bda440 45 BT.baud(9600);
JohanBeverini 2:c340f1bda440 46
JohanBeverini 2:c340f1bda440 47 pc.printf("hello word\n");
JohanBeverini 2:c340f1bda440 48 BT.printf("connection...\n");
JohanBeverini 2:c340f1bda440 49
Digixx 0:9410b3e6aea5 50 //values x,y,z
Digixx 0:9410b3e6aea5 51 int readings[3];
Digixx 0:9410b3e6aea5 52 //ID Buffer
Digixx 0:9410b3e6aea5 53 char buffer[3];
Digixx 0:9410b3e6aea5 54
Digixx 0:9410b3e6aea5 55 pc.printf("%c" ,13,10,13,10,13,10);
Digixx 0:9410b3e6aea5 56
Digixx 0:9410b3e6aea5 57 // do init stuff
Digixx 0:9410b3e6aea5 58 //Continuous mode, , 10Hz measurement rate.
Digixx 0:9410b3e6aea5 59 // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
Digixx 0:9410b3e6aea5 60 cmp.setDefault();
Digixx 0:9410b3e6aea5 61 //Wait some time(Need at least 5ms)
Digixx 0:9410b3e6aea5 62 wait(0.1);
Digixx 0:9410b3e6aea5 63 cmp.getAddress(buffer);
Digixx 0:9410b3e6aea5 64 pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
Digixx 0:9410b3e6aea5 65
Digixx 0:9410b3e6aea5 66 // These are here to test whether any of the initialization fails. It will print the failure
Digixx 0:9410b3e6aea5 67 if (acc.setPowerControl(0x00)){
Digixx 0:9410b3e6aea5 68 pc.printf("acc: didn't intitialize power control\n");
Digixx 0:9410b3e6aea5 69 return 0; }
Digixx 0:9410b3e6aea5 70 wait(.001);
Digixx 0:9410b3e6aea5 71
Digixx 0:9410b3e6aea5 72 //Full resolution, +/-16g, 4mg/LSB.
Digixx 0:9410b3e6aea5 73 if(acc.setDataFormatControl(0x0B)){
Digixx 0:9410b3e6aea5 74 pc.printf("didn't set data format\n");
Digixx 0:9410b3e6aea5 75 return 0; }
Digixx 0:9410b3e6aea5 76 wait(.001);
Digixx 0:9410b3e6aea5 77
Digixx 0:9410b3e6aea5 78 //3.2kHz data rate.
Digixx 0:9410b3e6aea5 79 if(acc.setDataRate(ADXL345_3200HZ)){
Digixx 0:9410b3e6aea5 80 pc.printf("didn't set data rate\n");
Digixx 0:9410b3e6aea5 81 return 0; }
Digixx 0:9410b3e6aea5 82 wait(.001);
Digixx 0:9410b3e6aea5 83
Digixx 0:9410b3e6aea5 84 //Measurement mode.
Digixx 0:9410b3e6aea5 85 if(acc.setPowerControl(MeasurementMode)) {
Digixx 0:9410b3e6aea5 86 pc.printf("didn't set the power control to measurement\n");
Digixx 0:9410b3e6aea5 87 return 0; }
Digixx 0:9410b3e6aea5 88
Digixx 0:9410b3e6aea5 89 pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
Digixx 0:9410b3e6aea5 90 pc.printf("%c" ,13,10);
Digixx 0:9410b3e6aea5 91
Digixx 0:9410b3e6aea5 92 //Set highest bandwidth.
Digixx 0:9410b3e6aea5 93 gyr.setLpBandwidth(LPFBW_256HZ);
Digixx 0:9410b3e6aea5 94 pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
Digixx 0:9410b3e6aea5 95 pc.printf("%c" ,13,10);
Digixx 0:9410b3e6aea5 96
Digixx 0:9410b3e6aea5 97 wait(1);
JohanBeverini 2:c340f1bda440 98
JohanBeverini 2:c340f1bda440 99 t.start();
JohanBeverini 2:c340f1bda440 100 Last = t.read_us();
JohanBeverini 2:c340f1bda440 101 count=0;
Digixx 0:9410b3e6aea5 102
Digixx 0:9410b3e6aea5 103 while (1) {
JohanBeverini 2:c340f1bda440 104 Now = t.read_us();
JohanBeverini 2:c340f1bda440 105 float delta = (float) (Now-Last)/1000000.0f;
JohanBeverini 2:c340f1bda440 106 if (delta>=0.1f && START==true) {
JohanBeverini 2:c340f1bda440 107
JohanBeverini 2:c340f1bda440 108 pc.printf("delta = %f (en s)\n",delta);
JohanBeverini 2:c340f1bda440 109
JohanBeverini 2:c340f1bda440 110 Last=Now;
JohanBeverini 2:c340f1bda440 111 count+=1;
Digixx 0:9410b3e6aea5 112
Digixx 0:9410b3e6aea5 113 cmp.readData(readings);
JohanBeverini 2:c340f1bda440 114 mag_x=((float)(int16_t)readings[0])*(0.92f/1000.0f); //valeur au nord de 470mGa (miliGauss)
JohanBeverini 2:c340f1bda440 115 mag_y=((float)(int16_t)readings[1])*(0.92f/1000.0f);
JohanBeverini 2:c340f1bda440 116 mag_z=((float)(int16_t)readings[2])*(0.92f/1000.0f);
JohanBeverini 2:c340f1bda440 117 pc.printf("C %+f %+f %+f (en Ga)",mag_x,mag_y,mag_z);
JohanBeverini 2:c340f1bda440 118 //wait(0.05);
Digixx 0:9410b3e6aea5 119
Digixx 0:9410b3e6aea5 120 acc.getOutput(readings);
JohanBeverini 2:c340f1bda440 121 acc_x=-((float)(int16_t)readings[0])/266.0f;
JohanBeverini 2:c340f1bda440 122 acc_y=-((float)(int16_t)readings[1])/256.0f;
JohanBeverini 2:c340f1bda440 123 acc_z=-((float)(int16_t)readings[2])/256.0f-0.1f;
JohanBeverini 2:c340f1bda440 124 pc.printf(" A %+f %+f %+f (en g)",acc_x,acc_y,acc_z);
JohanBeverini 2:c340f1bda440 125 //wait(0.05);
Digixx 0:9410b3e6aea5 126
JohanBeverini 2:c340f1bda440 127 gyr_x=(((float)(int16_t)gyr.getGyroX())/14.375f)+4.4f;
JohanBeverini 2:c340f1bda440 128 gyr_y=(((float)(int16_t)gyr.getGyroY())/14.375f);
JohanBeverini 2:c340f1bda440 129 gyr_z=(((float)(int16_t)gyr.getGyroZ())/14.375f)-1.6f;
JohanBeverini 2:c340f1bda440 130 pc.printf(" G %+f %+f %+f (en deg/s)",gyr_x,gyr_y,gyr_z);
Digixx 0:9410b3e6aea5 131 pc.printf("%c" ,13,10);
JohanBeverini 2:c340f1bda440 132 //wait(0.05);
JohanBeverini 2:c340f1bda440 133
JohanBeverini 2:c340f1bda440 134 float signe_z=1.0f;
JohanBeverini 2:c340f1bda440 135 if (acc_z<0) { signe_z=-1.0f; }
Digixx 0:9410b3e6aea5 136
JohanBeverini 2:c340f1bda440 137 //Theta = Roulis (en X)
JohanBeverini 2:c340f1bda440 138 if (abs(theta1*180.0f/PI)<80.0f){
JohanBeverini 2:c340f1bda440 139 theta = atan2(acc_y,sqrt(acc_x*acc_x+acc_z*acc_z)) ;
JohanBeverini 2:c340f1bda440 140 }
JohanBeverini 2:c340f1bda440 141 else {
JohanBeverini 2:c340f1bda440 142 theta = atan2(acc_y,signe_z*sqrt(acc_x*acc_x+acc_z*acc_z)) ;
JohanBeverini 2:c340f1bda440 143 }
JohanBeverini 2:c340f1bda440 144 //Phi = Tangage (en Y)
JohanBeverini 2:c340f1bda440 145 if (abs(psi1*180.0f/PI)<80.0f){
JohanBeverini 2:c340f1bda440 146 phi = atan2(acc_x,sqrt(acc_y*acc_y+acc_z*acc_z)) ;
JohanBeverini 2:c340f1bda440 147 }
JohanBeverini 2:c340f1bda440 148 else {
JohanBeverini 2:c340f1bda440 149 phi = atan2(acc_x,signe_z*sqrt(acc_y*acc_y+acc_z*acc_z)) ;
JohanBeverini 2:c340f1bda440 150 }
JohanBeverini 2:c340f1bda440 151 //Psi = Lacet (en Z)
JohanBeverini 2:c340f1bda440 152 //psi = atan2( (-mag_y*cos(phi) + mag_z*sin(phi) ) , (mag_x*cos(theta) + mag_y*sin(theta)*sin(phi)+ mag_z*sin(theta)*cos(phi)) ) ;
JohanBeverini 2:c340f1bda440 153 float signe_z_mag=1.0f;
JohanBeverini 2:c340f1bda440 154 if (mag_z<0) { signe_z_mag=-1.0f; }
JohanBeverini 2:c340f1bda440 155 if(mag_y>0)
JohanBeverini 2:c340f1bda440 156 {
JohanBeverini 2:c340f1bda440 157 psi = signe_z_mag*(PI/2.0f-(atan(mag_x/mag_y)));
JohanBeverini 2:c340f1bda440 158 }
JohanBeverini 2:c340f1bda440 159 else if (mag_y<0)
JohanBeverini 2:c340f1bda440 160 {
JohanBeverini 2:c340f1bda440 161 psi = signe_z_mag*(-PI/2.0f-(atan(mag_x/mag_y)));
JohanBeverini 2:c340f1bda440 162 }
JohanBeverini 2:c340f1bda440 163 else if (mag_y==0 && mag_x<0)
JohanBeverini 2:c340f1bda440 164 {
JohanBeverini 2:c340f1bda440 165 psi = PI;
JohanBeverini 2:c340f1bda440 166 }
JohanBeverini 2:c340f1bda440 167 else
JohanBeverini 2:c340f1bda440 168 {
JohanBeverini 2:c340f1bda440 169 psi = 0;
JohanBeverini 2:c340f1bda440 170 }
JohanBeverini 2:c340f1bda440 171 if(psi>=0){
JohanBeverini 2:c340f1bda440 172 psi=(psi-50.0f*PI/180.0f)*(180.0f/64.0f);
JohanBeverini 2:c340f1bda440 173 }
JohanBeverini 2:c340f1bda440 174 else {
JohanBeverini 2:c340f1bda440 175 psi=(psi+50.0f*PI/180.0f)*(180.0f/82.0f);
Digixx 0:9410b3e6aea5 176 }
Digixx 0:9410b3e6aea5 177
JohanBeverini 2:c340f1bda440 178
JohanBeverini 2:c340f1bda440 179 theta1=theta;
JohanBeverini 2:c340f1bda440 180 phi1=phi;
JohanBeverini 2:c340f1bda440 181 psi1=psi;
JohanBeverini 2:c340f1bda440 182
JohanBeverini 2:c340f1bda440 183 pc.printf("angles %+f %+f %+f (en deg)\n",theta*180.0f/PI,phi*180.0f/PI,psi*180.0f/PI);
JohanBeverini 2:c340f1bda440 184
JohanBeverini 2:c340f1bda440 185
JohanBeverini 2:c340f1bda440 186 //Filtrage passe bas
JohanBeverini 2:c340f1bda440 187 int N=2; //ordre du filtre
JohanBeverini 2:c340f1bda440 188 for (int i=0;i<N;i++){
JohanBeverini 2:c340f1bda440 189 x_x_filter[N-i]=x_x_filter[N-i-1];
JohanBeverini 2:c340f1bda440 190 y_x_filter[N-i]=y_x_filter[N-i-1];
JohanBeverini 2:c340f1bda440 191 z_x_filter[N-i]=z_x_filter[N-i-1];
JohanBeverini 2:c340f1bda440 192 }
JohanBeverini 2:c340f1bda440 193 x_x_filter[0]=theta;
JohanBeverini 2:c340f1bda440 194 y_x_filter[0]=phi;
JohanBeverini 2:c340f1bda440 195 z_x_filter[0]=psi;
JohanBeverini 2:c340f1bda440 196 for (int i=0;i<N;i++){
JohanBeverini 2:c340f1bda440 197 x_y_filter[N-i]=x_y_filter[N-i-1];
JohanBeverini 2:c340f1bda440 198 y_y_filter[N-i]=y_y_filter[N-i-1];
JohanBeverini 2:c340f1bda440 199 z_y_filter[N-i]=z_y_filter[N-i-1];
JohanBeverini 2:c340f1bda440 200 }
JohanBeverini 2:c340f1bda440 201 x_y_filter[0]=b_coef[0]*x_x_filter[0];
JohanBeverini 2:c340f1bda440 202 y_y_filter[0]=b_coef[0]*y_x_filter[0];
JohanBeverini 2:c340f1bda440 203 z_y_filter[0]=b_coef[0]*z_x_filter[0];
JohanBeverini 2:c340f1bda440 204 for (int i=1;i<=N;i++){
JohanBeverini 2:c340f1bda440 205 x_y_filter[0]+=b_coef[i]*x_x_filter[i]-a_coef[i]*x_y_filter[i];
JohanBeverini 2:c340f1bda440 206 y_y_filter[0]+=b_coef[i]*y_x_filter[i]-a_coef[i]*y_y_filter[i];
JohanBeverini 2:c340f1bda440 207 z_y_filter[0]+=b_coef[i]*z_x_filter[i]-a_coef[i]*z_y_filter[i];
JohanBeverini 2:c340f1bda440 208 }
JohanBeverini 2:c340f1bda440 209 theta_filtre=x_y_filter[0];
JohanBeverini 2:c340f1bda440 210 phi_filtre=y_y_filter[0];
JohanBeverini 2:c340f1bda440 211 psi_filtre=z_y_filter[0];
JohanBeverini 2:c340f1bda440 212 //theta_filtre=theta;
JohanBeverini 2:c340f1bda440 213 //phi_filtre=phi;
JohanBeverini 2:c340f1bda440 214 //psi_filtre=psi;
JohanBeverini 2:c340f1bda440 215
JohanBeverini 2:c340f1bda440 216 pc.printf("angles filtres %+f %+f %+f (en deg)\n",theta_filtre*180.0f/PI,phi_filtre*180.0f/PI,psi_filtre*180.0f/PI);
JohanBeverini 2:c340f1bda440 217
JohanBeverini 2:c340f1bda440 218 ///matrice d'Euler
JohanBeverini 2:c340f1bda440 219 ca=cos(theta_filtre); cb=cos(phi_filtre); cc=cos(psi_filtre);
JohanBeverini 2:c340f1bda440 220 sa=sin(theta_filtre); sb=sin(phi_filtre); sc=sin(psi_filtre);
JohanBeverini 2:c340f1bda440 221
JohanBeverini 2:c340f1bda440 222 matrice[0][0] = cc*cb;
JohanBeverini 2:c340f1bda440 223 matrice[0][1] = -sc*ca + cc*sb*sa;
JohanBeverini 2:c340f1bda440 224 matrice[0][2] = sc*sa + cc*sb*ca;
JohanBeverini 2:c340f1bda440 225 matrice[1][0] = sc*cb;
JohanBeverini 2:c340f1bda440 226 matrice[1][1] = cc*ca + sc*sb*sa;
JohanBeverini 2:c340f1bda440 227 matrice[1][2] = -cc*sa + sc*sb*ca;
JohanBeverini 2:c340f1bda440 228 matrice[2][0] = -sb;
JohanBeverini 2:c340f1bda440 229 matrice[2][1] = cb*sa;
JohanBeverini 2:c340f1bda440 230 matrice[2][2] = cb*ca;
JohanBeverini 2:c340f1bda440 231
JohanBeverini 2:c340f1bda440 232 for(int i=0; i<3; i++)
JohanBeverini 2:c340f1bda440 233 {
JohanBeverini 2:c340f1bda440 234 float temp = 0;
JohanBeverini 2:c340f1bda440 235 temp = acc_x * matrice[i][0] + acc_y * matrice[i][1] + acc_z * matrice[i][2];
JohanBeverini 2:c340f1bda440 236 resultat[i] = temp;
JohanBeverini 2:c340f1bda440 237 }
JohanBeverini 2:c340f1bda440 238
JohanBeverini 2:c340f1bda440 239 float poids = -1.0f;
JohanBeverini 2:c340f1bda440 240 if(resultat[2]<0){
JohanBeverini 2:c340f1bda440 241 poids = 1.0f;
JohanBeverini 2:c340f1bda440 242 }
JohanBeverini 2:c340f1bda440 243
JohanBeverini 2:c340f1bda440 244 pc.printf("apres Euler : %+f %+f %+f (en g)\n",resultat[0],resultat[1],resultat[2]+poids);
JohanBeverini 2:c340f1bda440 245
JohanBeverini 2:c340f1bda440 246 tableau_x[count-1]=resultat[0];
JohanBeverini 2:c340f1bda440 247 tableau_y[count-1]=resultat[1];
JohanBeverini 2:c340f1bda440 248 tableau_z[count-1]=resultat[2]+poids;
JohanBeverini 2:c340f1bda440 249
JohanBeverini 2:c340f1bda440 250 if (count>=10){
JohanBeverini 2:c340f1bda440 251 myled=!myled;
JohanBeverini 2:c340f1bda440 252 count=0;
JohanBeverini 2:c340f1bda440 253 float out_x =0;
JohanBeverini 2:c340f1bda440 254 float out_y =0;
JohanBeverini 2:c340f1bda440 255 float out_z =0;
JohanBeverini 2:c340f1bda440 256 for(int i=0;i<10;i++){
JohanBeverini 2:c340f1bda440 257 out_x+=tableau_x[i];
JohanBeverini 2:c340f1bda440 258 out_y+=tableau_y[i];
JohanBeverini 2:c340f1bda440 259 out_z+=tableau_z[i];
JohanBeverini 2:c340f1bda440 260 //BT.printf("%d;%d ",(int32_t)(tableau_x[i]*1000.0f),(int32_t)(tableau_y[i]*1000.0f));
JohanBeverini 2:c340f1bda440 261 }
JohanBeverini 2:c340f1bda440 262 out_x/=10.0f;
JohanBeverini 2:c340f1bda440 263 out_y/=10.0f;
JohanBeverini 2:c340f1bda440 264 out_z/=10.0f;
JohanBeverini 2:c340f1bda440 265
JohanBeverini 2:c340f1bda440 266 ///modifier ici
JohanBeverini 2:c340f1bda440 267 BT.printf("%f",0.25f); //out_x); //,(int8_t)(out_y*1000.0f));
JohanBeverini 2:c340f1bda440 268 pc.printf("\n%f ; %f\n\n",out_x,out_y);
JohanBeverini 2:c340f1bda440 269
JohanBeverini 2:c340f1bda440 270 START=false;
JohanBeverini 2:c340f1bda440 271
JohanBeverini 2:c340f1bda440 272 //BT.printf("E%f,%f,%f\n",out_x,out_y,out_z);
JohanBeverini 2:c340f1bda440 273
JohanBeverini 2:c340f1bda440 274 }
JohanBeverini 2:c340f1bda440 275 }
JohanBeverini 2:c340f1bda440 276
JohanBeverini 2:c340f1bda440 277 if (BT.readable()) {
JohanBeverini 2:c340f1bda440 278 char c = BT.getc();
JohanBeverini 2:c340f1bda440 279 if(c == '1') {
JohanBeverini 2:c340f1bda440 280 //BT.printf("\nOK\n");
JohanBeverini 2:c340f1bda440 281 START=true;
JohanBeverini 2:c340f1bda440 282 }
JohanBeverini 2:c340f1bda440 283 if(c == '0') {
JohanBeverini 2:c340f1bda440 284 //BT.printf("\nOK\n");
JohanBeverini 2:c340f1bda440 285 START=false;
JohanBeverini 2:c340f1bda440 286 }
JohanBeverini 2:c340f1bda440 287 }
JohanBeverini 2:c340f1bda440 288
Digixx 0:9410b3e6aea5 289 }
Digixx 0:9410b3e6aea5 290 }