ok
Dependencies: ADXL345_I2C HMC5843 mbed
main.cpp
- Committer:
- rulla
- Date:
- 2017-10-03
- Revision:
- 0:af8f98895e2a
File content as of revision 0:af8f98895e2a:
#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; }