Diff: main.cpp

Revision:

2:c340f1bda440

Parent:

1:18f68a0b0793

diff -r 18f68a0b0793 -r c340f1bda440 main.cpp
--- a/main.cpp	Fri Oct 07 21:42:29 2011 +0000
+++ b/main.cpp	Wed Mar 28 17:01:42 2018 +0000
@@ -2,16 +2,51 @@
 #include "HMC5843.h"
 #include "ADXL345.h"
 #include "ITG3200.h"
+#include <math.h>
+
+#define PI 3.14159265358979323846f
 
 DigitalOut myled(LED1);
 
-HMC5843 cmp(p28, p27);      // sda, scl
-ADXL345 acc(p28, p27);      // sda, scl
-ITG3200 gyr(p28, p27);      // sda, scl
-Serial pc(USBTX, USBRX);    // tx, rx
+HMC5843 cmp(D4, D5);      // sda, scl
+ADXL345 acc(D4, D5);      // sda, scl
+ITG3200 gyr(D4, D5);      // sda, scl
+
+Serial pc(SERIAL_TX, SERIAL_RX);    // tx, rx
+Serial BT(PA_9, PA_10); // tx, rx
+
+Timer t;
+
+float acc_x,acc_y,acc_z;
+float mag_x,mag_y,mag_z;
+float gyr_x,gyr_y,gyr_z;
+
+float theta,phi,psi;
+float theta1,phi1,psi1;
+
+double Last, Now;
+bool START = true;
+int count;
+
+    float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0};
+    float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0};
+    float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0};
+    float a_coef[3]={1.0000,   -0.3695,    0.1958};     //coefs pour filtre PB IIR2 à fréquence coupure normalisé 0.4
+    float b_coef[3]={0.2066,    0.4131,    0.2066};
+    float theta_filtre, phi_filtre, psi_filtre;
+    
+    float matrice[3][3], resultat[3];
+    float ca,cb,cc,sa,sb,sc;
+    float tableau_x[10], tableau_y[10], tableau_z[10];
+
 
 int main() {
-    pc.baud(115200);
+    pc.baud(9600);
+    BT.baud(9600); 
+  
+    pc.printf("hello word\n");
+    BT.printf("connection...\n");
+    
     //values x,y,z
     int readings[3];
     //ID Buffer
@@ -60,26 +95,196 @@
     pc.printf("%c" ,13,10);
 
     wait(1);
+    
+    t.start(); 
+    Last = t.read_us();
+    count=0;
 
     while (1) {
+     Now = t.read_us();
+     float delta = (float) (Now-Last)/1000000.0f;
+     if (delta>=0.1f && START==true) {
+        
+        pc.printf("delta = %f (en s)\n",delta);
+        
+        Last=Now;
+        count+=1;
 
         cmp.readData(readings);
-        pc.printf("C %+4i %+4i %+4i",(int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[2]);
-        wait(0.05);
+        mag_x=((float)(int16_t)readings[0])*(0.92f/1000.0f);    //valeur au nord de 470mGa (miliGauss)
+        mag_y=((float)(int16_t)readings[1])*(0.92f/1000.0f);
+        mag_z=((float)(int16_t)readings[2])*(0.92f/1000.0f);
+        pc.printf("C %+f %+f %+f (en Ga)",mag_x,mag_y,mag_z);
+        //wait(0.05);
 
         acc.getOutput(readings);
-        pc.printf(" A %+5i %+5i %+5i",(int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[2]);
-        wait(0.05);
+        acc_x=-((float)(int16_t)readings[0])/266.0f;
+        acc_y=-((float)(int16_t)readings[1])/256.0f;
+        acc_z=-((float)(int16_t)readings[2])/256.0f-0.1f;
+        pc.printf(" A %+f %+f %+f (en g)",acc_x,acc_y,acc_z);
+        //wait(0.05);
         
-        pc.printf(" G %+5i %+5i %+5i",(int16_t)gyr.getGyroX(),(int16_t)gyr.getGyroY(),(int16_t)gyr.getGyroZ());
+        gyr_x=(((float)(int16_t)gyr.getGyroX())/14.375f)+4.4f;
+        gyr_y=(((float)(int16_t)gyr.getGyroY())/14.375f);
+        gyr_z=(((float)(int16_t)gyr.getGyroZ())/14.375f)-1.6f;
+        pc.printf(" G %+f %+f %+f (en deg/s)",gyr_x,gyr_y,gyr_z);
         pc.printf("%c" ,13,10);        
-        wait(0.05);
+        //wait(0.05);
+        
+        float signe_z=1.0f;
+        if (acc_z<0) { signe_z=-1.0f; }
         
-        if (myled) {
-            myled=0;
-        } else {
-            myled=1;
+        //Theta = Roulis (en X)
+        if (abs(theta1*180.0f/PI)<80.0f){
+            theta = atan2(acc_y,sqrt(acc_x*acc_x+acc_z*acc_z)) ;
+        }
+        else {
+            theta = atan2(acc_y,signe_z*sqrt(acc_x*acc_x+acc_z*acc_z)) ;
+        }
+        //Phi = Tangage (en Y)
+        if (abs(psi1*180.0f/PI)<80.0f){
+            phi = atan2(acc_x,sqrt(acc_y*acc_y+acc_z*acc_z)) ;
+        } 
+        else {
+            phi = atan2(acc_x,signe_z*sqrt(acc_y*acc_y+acc_z*acc_z)) ; 
+        }
+        //Psi = Lacet (en Z)
+        //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)) ) ;
+        float signe_z_mag=1.0f;
+        if (mag_z<0) { signe_z_mag=-1.0f; }
+        if(mag_y>0)
+        {
+            psi = signe_z_mag*(PI/2.0f-(atan(mag_x/mag_y)));
+        }
+        else if (mag_y<0)
+        {
+            psi = signe_z_mag*(-PI/2.0f-(atan(mag_x/mag_y)));
+        }
+        else if (mag_y==0 && mag_x<0)
+        {
+            psi = PI;
+        }    
+        else
+        {
+            psi = 0;
+        }  
+        if(psi>=0){ 
+            psi=(psi-50.0f*PI/180.0f)*(180.0f/64.0f);
+        }
+        else {
+            psi=(psi+50.0f*PI/180.0f)*(180.0f/82.0f);
         }
         
+        
+        theta1=theta;
+        phi1=phi;
+        psi1=psi;
+        
+        pc.printf("angles %+f %+f %+f (en deg)\n",theta*180.0f/PI,phi*180.0f/PI,psi*180.0f/PI);
+        
+        
+        //Filtrage passe bas
+        int N=2; //ordre du filtre
+        for (int i=0;i<N;i++){
+            x_x_filter[N-i]=x_x_filter[N-i-1];
+            y_x_filter[N-i]=y_x_filter[N-i-1];
+            z_x_filter[N-i]=z_x_filter[N-i-1];
+        }
+        x_x_filter[0]=theta;
+        y_x_filter[0]=phi;
+        z_x_filter[0]=psi;
+        for (int i=0;i<N;i++){
+            x_y_filter[N-i]=x_y_filter[N-i-1];
+            y_y_filter[N-i]=y_y_filter[N-i-1];
+            z_y_filter[N-i]=z_y_filter[N-i-1];
+        }
+        x_y_filter[0]=b_coef[0]*x_x_filter[0];
+        y_y_filter[0]=b_coef[0]*y_x_filter[0];
+        z_y_filter[0]=b_coef[0]*z_x_filter[0];
+        for (int i=1;i<=N;i++){
+            x_y_filter[0]+=b_coef[i]*x_x_filter[i]-a_coef[i]*x_y_filter[i];
+            y_y_filter[0]+=b_coef[i]*y_x_filter[i]-a_coef[i]*y_y_filter[i];
+            z_y_filter[0]+=b_coef[i]*z_x_filter[i]-a_coef[i]*z_y_filter[i];
+        }
+        theta_filtre=x_y_filter[0];
+        phi_filtre=y_y_filter[0];
+        psi_filtre=z_y_filter[0];
+        //theta_filtre=theta;
+        //phi_filtre=phi;
+        //psi_filtre=psi;
+        
+        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);
+        
+        ///matrice d'Euler
+        ca=cos(theta_filtre); cb=cos(phi_filtre); cc=cos(psi_filtre);
+        sa=sin(theta_filtre); sb=sin(phi_filtre); sc=sin(psi_filtre);
+        
+        matrice[0][0] = cc*cb;
+        matrice[0][1] = -sc*ca + cc*sb*sa;
+        matrice[0][2] = sc*sa + cc*sb*ca;
+        matrice[1][0] = sc*cb;
+        matrice[1][1] = cc*ca + sc*sb*sa;
+        matrice[1][2] = -cc*sa + sc*sb*ca;
+        matrice[2][0] = -sb;
+        matrice[2][1] = cb*sa;
+        matrice[2][2] = cb*ca;
+        
+        for(int i=0; i<3; i++)
+        {
+            float temp = 0;
+            temp = acc_x * matrice[i][0] + acc_y * matrice[i][1] + acc_z * matrice[i][2];
+            resultat[i] = temp;
+        }
+        
+        float poids = -1.0f;
+        if(resultat[2]<0){
+            poids = 1.0f;
+        }
+        
+        pc.printf("apres Euler : %+f %+f %+f (en g)\n",resultat[0],resultat[1],resultat[2]+poids);
+        
+        tableau_x[count-1]=resultat[0];
+        tableau_y[count-1]=resultat[1];
+        tableau_z[count-1]=resultat[2]+poids;
+        
+        if (count>=10){
+            myled=!myled;
+            count=0;
+            float out_x =0;
+            float out_y =0;
+            float out_z =0;
+            for(int i=0;i<10;i++){
+                out_x+=tableau_x[i];
+                out_y+=tableau_y[i];
+                out_z+=tableau_z[i];
+                //BT.printf("%d;%d ",(int32_t)(tableau_x[i]*1000.0f),(int32_t)(tableau_y[i]*1000.0f));
+            }
+            out_x/=10.0f;
+            out_y/=10.0f;
+            out_z/=10.0f;
+
+///modifier ici
+            BT.printf("%f",0.25f); //out_x); //,(int8_t)(out_y*1000.0f));
+            pc.printf("\n%f ; %f\n\n",out_x,out_y);
+            
+            START=false;
+            
+            //BT.printf("E%f,%f,%f\n",out_x,out_y,out_z);
+            
+        }
+      }
+      
+      if (BT.readable()) {
+        char c = BT.getc();
+        if(c == '1') {
+            //BT.printf("\nOK\n");
+            START=true;
+        }
+        if(c == '0') {
+            //BT.printf("\nOK\n");
+            START=false;
+        }
+      }
+    
     }
 }