ok
Dependencies: ADXL345_I2C HMC5843 mbed
Diff: main.cpp
- Revision:
- 0:af8f98895e2a
diff -r 000000000000 -r af8f98895e2a main.cpp --- /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