File content as of revision 1:d47d60db8df3:

#include "mbed.h"
#include "ADXL345_I2C.h"
#include "HMC5843.h"

#define N_VAR 15

HMC5843 compass(p28, p27);
ADXL345_I2C accelerometer(p28, p27);
Serial pc(USBTX, USBRX);

int l;
float Complementary(float newAngle, int looptime);
float Complementary2(float newAngle, int looptime);
double kalman_update(int index,double measurement);
float kalmanCalculate(float newAngle, int looptime);
void accMagInit(void);
void ahrs(void);
void kalman_init(void);

double q[N_VAR]; //process noise covariance
double r[N_VAR]; //measurement noise covariance
double value[N_VAR]; //value
double p[N_VAR]; //estimation error covariance
double k[N_VAR]; //kalman gain

float Q_angle  =  0.01; //0.001
float R_angle  =  0.02;  //0.03
float x_angle = 0.0;
float x_bias = 0.0;
float P_00 = 1.0, P_01 = 0.0, P_10 = 0.0, P_11 = 1.0;
float  y, S;
float K_0, K_1;

int i=0;
int ax=0,ay=0,az=0,bx=0,by=0,bz=0, ax0=0,ay0=0,az0=0;
int mx=0,my=0,mz=0,bmx=0,bmy=0,bmz=0, mx0=0,my0=0,mz0=0;
float x1=0,y1=0,z1=0,x2=0,y2=0,z2=0;
double xd,yd,zd;
double aax,aay,aaz;
int cnt=0;
float thr=10.0;
    double ca; double cb; double cc ;
    double sa ; double sb ; double sc;
    
int main()
{
    pc.baud(115200);

    kalman_init();
    accMagInit();

    while (1) {
        ahrs();
    }
}


double kalman_update(int index,double measurement)
{
    //prediction update
    //omit x = x
    p[index]+= q[index];
    //measurement update
    k[index] = p[index] / (p[index] + r[index]);
    value[index] += k[index] * (measurement - value[index]);
    p[index] *= (1.0 - k[index]);
    //fprintf(out,"%f %f \r\n",p,k);
    return value[index];
}

void kalman_init(void)
{
    float s1,s2;
    //pc.scanf("Q=%f r=%f\n",&s1,&s2);
    for(l=0; l<N_VAR; l++) {
        q[l]=0.01; //process noise covariance
        r[l]=0.8; //measurement noise covariance
        p[l]=0.0; //estimation error covariance
        k[l]=0.0; //kalman gain
        value[l]=0.0;
    }
}

float kalmanCalculate(float newAngle, int looptime)
{
    float dt =(float)((looptime)/1000.0);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    //fprintf(out,"%f %f %f %f %f %f %f\r\n",P_00,P_01,P_10,P_11,K_0,K_1,S);
    return x_angle*1.5;
}


void accMagInit(void)
{

    int breadings[3] = {0, 0, 0};
    int bhmcreadings[3];
    char buffer[3];
    pc.printf("Starting ADXL345 test...\n");
    pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
    accelerometer.setPowerControl(0x00);
    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
    //3.2kHz data rate.
    accelerometer.setDataRate(ADXL345_3200HZ);
     accelerometer.setPowerControl(0x08);
    compass.getAddress(buffer);
    pc.printf("Magnetic Compass  Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
    compass.setDefault();
    wait(.1);
    for(i=0; i<50; i++) {
        accelerometer.getOutput(breadings);
        //13-bit, sign extended values.
        compass.readData(bhmcreadings);
        wait(0.1);
        ax+=(int16_t)(breadings[0]);
        ay+=(int16_t)(breadings[1]);
        az+=(int16_t)(breadings[2]);
        mx+=(int16_t)(bhmcreadings[0]);
        my+=(int16_t)(bhmcreadings[1]);
        mz+=(int16_t)(bhmcreadings[2]);
    }
    ax0=ax/50;
    ay0=ay/50;
    az0=az/50;
    mx0=mx/50;
    my0=my/50;
    mz0=mz/50;
    ax=0;
    ay=0;
    az=0;
    mx=0;
    my=0;
    mz=0;
}

void ahrs(void)
{
cnt=0;
    int readings[3] = {0, 0, 0};
    int hmcreadings[3];
   // int creadings[3] = {0, 0, 0};
   // int bhmcreadings[3];
    accelerometer.getOutput(readings);
    compass.readData(hmcreadings);
    //13-bit, sign extended values.
    cnt++;
    ax+=(int16_t)(readings[0]);
    ay+=(int16_t)(readings[1]);
    az+=(int16_t)(readings[2]);
    ax-=ax0;
    ay-=ay0;
    az-=az0;
    mx+=(int16_t)(hmcreadings[0]);
    my+=(int16_t)(hmcreadings[1]);
    mz+=(int16_t)(hmcreadings[2]);

    mx-=mx0;
    my-=my0;
    mz-=mz0;
  
    wait(0.1);
  
  /*
    accelerometer.getOutput(creadings);
    compass.readData(bhmcreadings);
    //13-bit, sign extended values.
    cnt++;
    bx+=(int16_t)(creadings[0]);
    by+=(int16_t)(creadings[1]);
    bz+=(int16_t)(creadings[2]);
    bx-=ax0;
    by-=ay0;
    bz-=az0;
    bmx+=(int16_t)(bhmcreadings[0]);
    bmy+=(int16_t)(bhmcreadings[1]);
    bmz+=(int16_t)(bhmcreadings[2]);
    bmx-=mx0;
    bmy-=my0;
    bmz-=mz0;
    wait(0.1);
    

//            if(cnt%10==0) {
    //   if(cnt==2) {

    if((bx-ax)<thr) x1=0;
    if((by-ay)<thr) y1=0;
    if((bz-az)<thr) z1=0;
    xd=((float)(ax+bx)/cnt);
    yd=((float)(ay+by)/cnt);
    zd=((float)(az+bz)/cnt);
    x1+=kalman_update(3,xd);
    x2+=kalman_update(6,x1);
    y1+= kalman_update(4,yd);
    y2+=kalman_update(7,y1);
    z1+= kalman_update(5,zd);
    z2+=kalman_update(8,z1);
    // pc.printf("%f %f %f\n",x2,y2,z2);
    */
    //pc.printf("%d %d %d\n",ax,ay,az);
    

    /*
    xd=(float)(ax)*488.288700694;
    yd=(float)(ay)*488.288700694;
    zd=(float)(az)*0.0488288700694;
    */
     xd=(double)(ax);
    yd=(double)(ay);
    zd=(double)(az);
    
    
    //Roll, Pitch
   // aax=(double)xd;
    //aay=(double)yd;
    //aaz=(double)zd;
    
    aax=kalman_update(9,xd); //Kalman to accelerometer
    aay=kalman_update(10,yd);
    aaz=kalman_update(11,-zd);   

/*
    x1+=kalman_update(3,aax);
    y1+= kalman_update(4,aay);
    z1+= kalman_update(5,aaz);
        
    if(aax<=1.6420 && aax>=-1.6420 ) x1=0.0;
    if(aay<=1.2315 && aay>=-1.1325 ) y1=0.0;
    if(aaz<=8.374 && aaz>=-8.374 ) z1=0.0;

    x2+=kalman_update(6,x1);
    y2+=kalman_update(7,y1);
    z2+=kalman_update(8,z1);
    
    
    if(aax<0.01 && aax>-0.01 ) x1=0;
    if(aay<0.0075 && aay>-0.0075 ) y1=0;
    if(aaz<0.051 && aaz>-0.051 ) z1=0;
    
    x1+=aax;
    x2+=kalman_update(6,x1);
    y1+=aay;
    y2+=kalman_update(7,y1);
    z1+= aaz;
    z2+=kalman_update(8,z1);
*/     
//double normA=sqrt(pow(aax,2)+pow(aay,2)+pow(aaz,2));
//aax/=normA;
//aay/=normA;
//aaz/=normA;  
  //  double xAngle = atan2( aax , (sqrt(pow(aay,2) + pow(aaz,2))));
    //double yAngle = atan2( aay , (sqrt(pow(aax,2) + pow(aaz,2))));
 // double yAngle = atan2(  (sqrt(pow(aax,2) + pow(aay,2))), aaz);
   // double zAngle = atan2( sqrt(pow(aax,2) + pow(aay,2)) , aaz);
   
   double xAngle=-0.00609*aay;
   double yAngle=0.00609*aax;   
    //double xAngle=-aay;
   //double yAngle=aax;   
    //double yAngle=atan2(aay,aaz); //Roll
    //double xAngle=atan(-aax/(aay*sin(yAngle)+aaz*cos(yAngle)));
    cnt=0;
    ax=0;
    ay=0;
    az=0;
    bx=0;
    by=0;
    bz=0;
    ////////Tilt compensated heading
    /*
    double mmx=(mx+bmx)*0.5;
    double mmy=(my+bmy)*0.5;
    double mmz=(mz+bmz)*0.5; //Mag vector components
*/

       double mmx=(double)(mx);
    double mmy=(double)(my);
    double mmz=(double)(mz);
//double normM=sqrt(pow(mmx,2)+pow(mmy,2)+pow(mmz,2));
//mmx/=normM;
//mmy/=normM;
//mmz/=normM;
double fx=kalman_update(0,xAngle);  //Kalman to angles
double fy=kalman_update(1,yAngle);
double fz=0.0;

//double fx=xAngle;   //PITCH
//double fy=yAngle;   //ROLL

    //double yh=mmy*cos(fx)+mmz*sin(fx);
  //  double xh=mmx*sin(fy)*sin(fx)+mmy*cos(fy)-mmz*sin(fy)*cos(fx);
      //double xh=mmy*sin(fy)*sin(fx)+mmx*cos(fy)+mmz*sin(fy)*cos(fx);
      //double yh=mmy*cos(fx)-mmz*sin(fx);
    //double xh=mmx*sin(fy)*sin(fx)+mmy*cos(fy)-mmz*sin(fy)*cos(fx);
    //double yh=mmx*cos(fx)+mmz*sin(fx);  

    ca = cos(fx);//theta
     cb = cos(fy);//phi
     sa = sin(fx);
     sb = sin(fy);
  
    double yh=mmz*sb-mmy*cb;
    double xh=+mmx*ca+mmy*sa*sb+mmz*sa*cb;  
  //  double normM=sqrt(pow(xh,2)+pow(yh,2));
//xh/=normM;
//yh/=normM;
//    double head=0.3*atan2(yh,xh);
    double head=atan2(yh,xh);
    //double head=atan2(mmy,mmx);
    

    const float pi=3.14;
    //  if (fz > pi) fz -= (2.0*pi);
    //if (fz < -pi)       fz += (2.0*pi);
    //if (fz < 0.0)      fz += 2.0*pi;
  
 //head=atan2(aay,(sqrt(pow(aax,2) + pow(aaz,2))));

/*
   double xh=mmx*cos(fx)+mmy*sin(fx)*sin(fy)+mmz*cos(fx)*sin(fy);
    double yh=mmy*cos(fy)-mmz*sin(fy);
    double head=atan2(-yh,xh);
*/
fz=kalman_update(2,head);
//fz=head;
//double fz=head;

/*
     cc = cos(fz);
     sc = sin(fz);
    double  x=1.0,y=1.0,z=1.0;
    double  xrot=x2+cb*cc*x+(-cc*sa*sb-ca*sc)*y+(ca*cc*sb-sa*sc)*z;
    double  yrot=y2+cb*sc*x+(ca*cc-sa*sb*sc)*y+(cc*sa+ca*sb*sc)*z;
    double  zrot=z2+sb*x-cb*sa*y+ca*cb*z;
   
    xrot=cb*cc*x+(-cc*sa*sb-ca*sc)*y+(ca*cc*sb-sa*sc)*z;
    yrot=cb*sc*x+(ca*cc-sa*sb*sc)*y+(cc*sa+ca*sb*sc)*z;
    zrot=sb*x-cb*sa*y+ca*cb*z;
    */
    //     pc.printf("%f %f %f\n",xrot,yrot,zrot);
   pc.printf("%f %f %f %f %f %f %f %f %f\n",fx,fy,fz,mmx,mmy,atan2(mmy,mmx),aax,aay,aaz);
 //   pc.printf("%f %f %f %f %f %f %f %f %f\n",fx,fy,fz,x2,y2,z2,kalman_update(9,x2),kalman_update(10,y2),kalman_update(11,z2));

    mx=0;
    my=0;
    mz=0;
    bmx=0;
    bmy=0;
    bmz=0;
}