main.cpp

00001 #include "mbed.h"
00002 #include "HMC5843.h"
00003 #include "ADXL345.h"
00004 #include "ITG3200.h"
00005 #include <math.h>
00006 
00007 #define PI 3.14159265358979323846f
00008 
00009 DigitalOut myled(LED1);
00010 
00011 HMC5843 cmp(D4, D5);      // sda, scl
00012 ADXL345 acc(D4, D5);      // sda, scl
00013 ITG3200 gyr(D4, D5);      // sda, scl
00014 
00015 Serial pc(SERIAL_TX, SERIAL_RX);    // tx, rx
00016 Serial BT(PA_9, PA_10); // tx, rx
00017 
00018 Timer t;
00019 
00020 float acc_x,acc_y,acc_z;
00021 float mag_x,mag_y,mag_z;
00022 float gyr_x,gyr_y,gyr_z;
00023 
00024 float theta,phi,psi;
00025 float theta1,phi1,psi1;
00026 
00027 double Last, Now;
00028 bool START = true;
00029 int count;
00030 
00031     float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0};
00032     float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0};
00033     float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0};
00034     float a_coef[3]={1.0000,   -0.3695,    0.1958};     //coefs pour filtre PB IIR2 à fréquence coupure normalisé 0.4
00035     float b_coef[3]={0.2066,    0.4131,    0.2066};
00036     float theta_filtre, phi_filtre, psi_filtre;
00037     
00038     float matrice[3][3], resultat[3];
00039     float ca,cb,cc,sa,sb,sc;
00040     float tableau_x[10], tableau_y[10], tableau_z[10];
00041 
00042 
00043 int main() {
00044     pc.baud(9600);
00045     BT.baud(9600); 
00046   
00047     pc.printf("hello word\n");
00048     BT.printf("connection...\n");
00049     
00050     //values x,y,z
00051     int readings[3];
00052     //ID Buffer
00053     char buffer[3];
00054     
00055     pc.printf("%c" ,13,10,13,10,13,10);
00056     
00057     // do init stuff
00058     //Continuous mode, , 10Hz measurement rate.
00059     // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
00060     cmp.setDefault();
00061     //Wait some time(Need at least 5ms)
00062     wait(0.1);
00063     cmp.getAddress(buffer);
00064     pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
00065  
00066     // These are here to test whether any of the initialization fails. It will print the failure
00067     if (acc.setPowerControl(0x00)){
00068          pc.printf("acc: didn't intitialize power control\n"); 
00069          return 0;  }
00070     wait(.001);
00071     
00072     //Full resolution, +/-16g, 4mg/LSB.
00073     if(acc.setDataFormatControl(0x0B)){
00074         pc.printf("didn't set data format\n");
00075         return 0;  }
00076     wait(.001);
00077      
00078     //3.2kHz data rate.
00079     if(acc.setDataRate(ADXL345_3200HZ)){
00080         pc.printf("didn't set data rate\n");
00081         return 0;    }
00082     wait(.001);
00083      
00084     //Measurement mode.
00085     if(acc.setPowerControl(MeasurementMode)) {
00086         pc.printf("didn't set the power control to measurement\n"); 
00087         return 0;   } 
00088 
00089     pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
00090     pc.printf("%c" ,13,10);
00091 
00092     //Set highest bandwidth.
00093     gyr.setLpBandwidth(LPFBW_256HZ);
00094     pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
00095     pc.printf("%c" ,13,10);
00096 
00097     wait(1);
00098     
00099     t.start(); 
00100     Last = t.read_us();
00101     count=0;
00102 
00103     while (1) {
00104      Now = t.read_us();
00105      float delta = (float) (Now-Last)/1000000.0f;
00106      if (delta>=0.1f && START==true) {
00107         
00108         pc.printf("delta = %f (en s)\n",delta);
00109         
00110         Last=Now;
00111         count+=1;
00112 
00113         cmp.readData(readings);
00114         mag_x=((float)(int16_t)readings[0])*(0.92f/1000.0f);    //valeur au nord de 470mGa (miliGauss)
00115         mag_y=((float)(int16_t)readings[1])*(0.92f/1000.0f);
00116         mag_z=((float)(int16_t)readings[2])*(0.92f/1000.0f);
00117         pc.printf("C %+f %+f %+f (en Ga)",mag_x,mag_y,mag_z);
00118         //wait(0.05);
00119 
00120         acc.getOutput(readings);
00121         acc_x=-((float)(int16_t)readings[0])/266.0f;
00122         acc_y=-((float)(int16_t)readings[1])/256.0f;
00123         acc_z=-((float)(int16_t)readings[2])/256.0f-0.1f;
00124         pc.printf(" A %+f %+f %+f (en g)",acc_x,acc_y,acc_z);
00125         //wait(0.05);
00126         
00127         gyr_x=(((float)(int16_t)gyr.getGyroX())/14.375f)+4.4f;
00128         gyr_y=(((float)(int16_t)gyr.getGyroY())/14.375f);
00129         gyr_z=(((float)(int16_t)gyr.getGyroZ())/14.375f)-1.6f;
00130         pc.printf(" G %+f %+f %+f (en deg/s)",gyr_x,gyr_y,gyr_z);
00131         pc.printf("%c" ,13,10);        
00132         //wait(0.05);
00133         
00134         float signe_z=1.0f;
00135         if (acc_z<0) { signe_z=-1.0f; }
00136         
00137         //Theta = Roulis (en X)
00138         if (abs(theta1*180.0f/PI)<80.0f){
00139             theta = atan2(acc_y,sqrt(acc_x*acc_x+acc_z*acc_z)) ;
00140         }
00141         else {
00142             theta = atan2(acc_y,signe_z*sqrt(acc_x*acc_x+acc_z*acc_z)) ;
00143         }
00144         //Phi = Tangage (en Y)
00145         if (abs(psi1*180.0f/PI)<80.0f){
00146             phi = atan2(acc_x,sqrt(acc_y*acc_y+acc_z*acc_z)) ;
00147         } 
00148         else {
00149             phi = atan2(acc_x,signe_z*sqrt(acc_y*acc_y+acc_z*acc_z)) ; 
00150         }
00151         //Psi = Lacet (en Z)
00152         //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)) ) ;
00153         float signe_z_mag=1.0f;
00154         if (mag_z<0) { signe_z_mag=-1.0f; }
00155         if(mag_y>0)
00156         {
00157             psi = signe_z_mag*(PI/2.0f-(atan(mag_x/mag_y)));
00158         }
00159         else if (mag_y<0)
00160         {
00161             psi = signe_z_mag*(-PI/2.0f-(atan(mag_x/mag_y)));
00162         }
00163         else if (mag_y==0 && mag_x<0)
00164         {
00165             psi = PI;
00166         }    
00167         else
00168         {
00169             psi = 0;
00170         }  
00171         if(psi>=0){ 
00172             psi=(psi-50.0f*PI/180.0f)*(180.0f/64.0f);
00173         }
00174         else {
00175             psi=(psi+50.0f*PI/180.0f)*(180.0f/82.0f);
00176         }
00177         
00178         
00179         theta1=theta;
00180         phi1=phi;
00181         psi1=psi;
00182         
00183         pc.printf("angles %+f %+f %+f (en deg)\n",theta*180.0f/PI,phi*180.0f/PI,psi*180.0f/PI);
00184         
00185         
00186         //Filtrage passe bas
00187         int N=2; //ordre du filtre
00188         for (int i=0;i<N;i++){
00189             x_x_filter[N-i]=x_x_filter[N-i-1];
00190             y_x_filter[N-i]=y_x_filter[N-i-1];
00191             z_x_filter[N-i]=z_x_filter[N-i-1];
00192         }
00193         x_x_filter[0]=theta;
00194         y_x_filter[0]=phi;
00195         z_x_filter[0]=psi;
00196         for (int i=0;i<N;i++){
00197             x_y_filter[N-i]=x_y_filter[N-i-1];
00198             y_y_filter[N-i]=y_y_filter[N-i-1];
00199             z_y_filter[N-i]=z_y_filter[N-i-1];
00200         }
00201         x_y_filter[0]=b_coef[0]*x_x_filter[0];
00202         y_y_filter[0]=b_coef[0]*y_x_filter[0];
00203         z_y_filter[0]=b_coef[0]*z_x_filter[0];
00204         for (int i=1;i<=N;i++){
00205             x_y_filter[0]+=b_coef[i]*x_x_filter[i]-a_coef[i]*x_y_filter[i];
00206             y_y_filter[0]+=b_coef[i]*y_x_filter[i]-a_coef[i]*y_y_filter[i];
00207             z_y_filter[0]+=b_coef[i]*z_x_filter[i]-a_coef[i]*z_y_filter[i];
00208         }
00209         theta_filtre=x_y_filter[0];
00210         phi_filtre=y_y_filter[0];
00211         psi_filtre=z_y_filter[0];
00212         //theta_filtre=theta;
00213         //phi_filtre=phi;
00214         //psi_filtre=psi;
00215         
00216         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);
00217         
00218         ///matrice d'Euler
00219         ca=cos(theta_filtre); cb=cos(phi_filtre); cc=cos(psi_filtre);
00220         sa=sin(theta_filtre); sb=sin(phi_filtre); sc=sin(psi_filtre);
00221         
00222         matrice[0][0] = cc*cb;
00223         matrice[0][1] = -sc*ca + cc*sb*sa;
00224         matrice[0][2] = sc*sa + cc*sb*ca;
00225         matrice[1][0] = sc*cb;
00226         matrice[1][1] = cc*ca + sc*sb*sa;
00227         matrice[1][2] = -cc*sa + sc*sb*ca;
00228         matrice[2][0] = -sb;
00229         matrice[2][1] = cb*sa;
00230         matrice[2][2] = cb*ca;
00231         
00232         for(int i=0; i<3; i++)
00233         {
00234             float temp = 0;
00235             temp = acc_x * matrice[i][0] + acc_y * matrice[i][1] + acc_z * matrice[i][2];
00236             resultat[i] = temp;
00237         }
00238         
00239         float poids = -1.0f;
00240         if(resultat[2]<0){
00241             poids = 1.0f;
00242         }
00243         
00244         pc.printf("apres Euler : %+f %+f %+f (en g)\n",resultat[0],resultat[1],resultat[2]+poids);
00245         
00246         tableau_x[count-1]=resultat[0];
00247         tableau_y[count-1]=resultat[1];
00248         tableau_z[count-1]=resultat[2]+poids;
00249         
00250         if (count>=10){
00251             myled=!myled;
00252             count=0;
00253             float out_x =0;
00254             float out_y =0;
00255             float out_z =0;
00256             for(int i=0;i<10;i++){
00257                 out_x+=tableau_x[i];
00258                 out_y+=tableau_y[i];
00259                 out_z+=tableau_z[i];
00260                 //BT.printf("%d;%d ",(int32_t)(tableau_x[i]*1000.0f),(int32_t)(tableau_y[i]*1000.0f));
00261             }
00262             out_x/=10.0f;
00263             out_y/=10.0f;
00264             out_z/=10.0f;
00265 
00266 ///modifier ici
00267             BT.printf("%f",0.25f); //out_x); //,(int8_t)(out_y*1000.0f));
00268             pc.printf("\n%f ; %f\n\n",out_x,out_y);
00269             
00270             START=false;
00271             
00272             //BT.printf("E%f,%f,%f\n",out_x,out_y,out_z);
00273             
00274         }
00275       }
00276       
00277       if (BT.readable()) {
00278         char c = BT.getc();
00279         if(c == '1') {
00280             //BT.printf("\nOK\n");
00281             START=true;
00282         }
00283         if(c == '0') {
00284             //BT.printf("\nOK\n");
00285             START=false;
00286         }
00287       }
00288     
00289     }
00290 }