Diff: main.cpp

Revision:

0:af8f98895e2a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 03 16:37:07 2017 +0000
@@ -0,0 +1,364 @@
+#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;
+}
\ No newline at end of file