File content as of revision 3:15f5e8de8533:

#include "mbed.h"
#include "HMC5843.h"
#include "ADXL345.h"
#include "ITG3200.h"
#include <math.h>

#define PI 3.14159265358979323846f

DigitalOut myled(LED1);

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(9600);
    BT.baud(9600); 
  
    pc.printf("hello word\n");
    BT.printf("connection...\n");
    
    //values x,y,z
    int readings[3];
    //ID Buffer
    char buffer[3];
    
    pc.printf("%c" ,13,10,13,10,13,10);
    
    // do init stuff
    //Continuous mode, , 10Hz measurement rate.
    // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
    cmp.setDefault();
    //Wait some time(Need at least 5ms)
    wait(0.1);
    cmp.getAddress(buffer);
    pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
 
    // These are here to test whether any of the initialization fails. It will print the failure
    if (acc.setPowerControl(0x00)){
         pc.printf("acc: didn't intitialize power control\n"); 
         return 0;  }
    wait(.001);
    
    //Full resolution, +/-16g, 4mg/LSB.
    if(acc.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\n");
        return 0;  }
    wait(.001);
     
    //3.2kHz data rate.
    if(acc.setDataRate(ADXL345_3200HZ)){
        pc.printf("didn't set data rate\n");
        return 0;    }
    wait(.001);
     
    //Measurement mode.
    if(acc.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n"); 
        return 0;   } 

    pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
    pc.printf("%c" ,13,10);

    //Set highest bandwidth.
    gyr.setLpBandwidth(LPFBW_256HZ);
    pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
    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);
        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);
        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);
        
        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);
        
        float signe_z=1.0f;
        if (acc_z<0) { signe_z=-1.0f; }
        
        //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;
        }
      }
    
    }
}