Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 40:869f3791a3e2
- Parent:
- 38:acc7cdcf56dd
- Child:
- 42:24c93e42c3f4
- Child:
- 43:f0dd73430192
--- a/main.cpp Mon Mar 22 02:37:50 2021 +0000 +++ b/main.cpp Tue Mar 23 06:37:24 2021 +0000 @@ -50,6 +50,7 @@ Matrix D(3,3); int loop_count = 0; +int obs_count = 0; float att_dt = 0.01; float rc[16]; float pitch = 0.0; @@ -63,6 +64,14 @@ float dynacc_x,dynacc_y,dynacc_z; float gyro_x,gyro_y,gyro_z; float mag_x,mag_y,mag_z; + +float LPacc_x=0.0; +float LPacc_y=0.0; +float LPacc_z=0.0; +float LPmag_x = 0.0; +float LPmag_y = 0.0; +float LPmag_z = 0.0;; + int out1, out2; float scaledServoOut[3]= {0,0}; float scaledMotorOut[1]= {-1}; @@ -71,10 +80,16 @@ float accnorm; float magnorm; +float LPaccnorm; +float LPmagnorm; float accrefnorm; float magrefnorm; + float val_thmg = 0.0; +float th_mg = 0.0; +float dynaccnorm = 0.0; +float accnormerr = 0.0; float accref[3] = {0, 0, -0.98}; float magref[3] = {0.65, 0, 0.75}; @@ -86,7 +101,8 @@ //magcal.getExtremes(&magmin[0],&magmax[0]); //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]); //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]); - sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); + //pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); + pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%d \r\n",userButton.read()); //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); @@ -140,14 +156,40 @@ mag_y = -magval[1]/magbias[3]; mag_z = -magval[2]/magbias[3]; + float lpc_acc = 0.15; + LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x; + LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y; + LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z; + + float lpc_mag = 0.15; + LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x; + LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y; + LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z; + accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z); magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z); + LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z); + LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z); accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]); magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]); - calcMagRef(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm); + calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm); + + } void updateBetweenMeasures(){ + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix B(4,3); + B << q1 << q2 << q3 + <<-q0 << q3 <<-q2 + <<-q3 <<-q0 << q1 + << q2 <<-q1 <<-q0; + B *= 0.5f; + Matrix phi(4,4); phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt @@ -158,19 +200,22 @@ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); - float q0 = qhat.getNumber( 1, 1 ); - float q1 = qhat.getNumber( 2, 1 ); - float q2 = qhat.getNumber( 3, 1 ); - float q3 = qhat.getNumber( 4, 1 ); - - Matrix B(4,3); - B << q1 << q2 << q3 - <<-q0 << q3 <<-q2 - <<-q3 <<-q0 << q1 - << q2 <<-q1 <<-q0; - B *= 0.5f; Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); + q0 = qhat.getNumber( 1, 1 ); + q1 = qhat.getNumber( 2, 1 ); + q2 = qhat.getNumber( 3, 1 ); + q3 = qhat.getNumber( 4, 1 ); + + D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); + D.add(1,2,2*(q1*q2 + q0*q3)); + D.add(1,3,2*(q1*q3 - q0*q2)); + D.add(2,1,2*(q1*q2 - q0*q3)); + D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); + D.add(2,3,2*(q2*q3 + q0*q1)); + D.add(3,1,2*(q1*q3 + q0*q2)); + D.add(3,2,2*(q2*q3 - q0*q1)); + D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); } @@ -222,16 +267,6 @@ << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); - D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); - D.add(1,2,2*(q1*q2 + q0*q3)); - D.add(1,3,2*(q1*q3 - q0*q2)); - D.add(2,1,2*(q1*q2 - q0*q3)); - D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); - D.add(2,3,2*(q2*q3 + q0*q1)); - D.add(3,1,2*(q1*q3 + q0*q2)); - D.add(3,2,2*(q2*q3 - q0*q1)); - D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); - Matrix K(4,3); K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); @@ -310,9 +345,9 @@ } void calcDynAcc(){ - dynacc_x = acc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]); - dynacc_y = acc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]); - dynacc_z = acc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]); + dynacc_x = LPacc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]); + dynacc_y = LPacc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]); + dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]); } void execCalibration(){ @@ -440,6 +475,7 @@ if(loop_count > 10){ writeSdcard(); loop_count = 1; + obs_count = 0; }else{ loop_count +=1; } @@ -521,17 +557,18 @@ getIMUval(); updateBetweenMeasures(); calcDynAcc(); - float th_mg = acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm); - float dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z); - + th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg); + dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z); + accnormerr = abs(LPaccnorm-accrefnorm); //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref)); - int ang_th = abs(th_mg-val_thmg)<0.005; - int dyn_th = dynaccnorm < 0.01/4; - int norm_th = abs(accnorm-accrefnorm)< 0.01/6.0; + int ang_th = th_mg<0.01; + int dyn_th = dynaccnorm < 0.01; + int norm_th = accnormerr< 0.01; //pc.printf("%d %d %d %f %f %f\r\n",ang_th,dyn_th,norm_th,abs(th_mg-val_thmg),dynaccnorm ,abs(accnorm-accrefnorm)); - if(ang_th+dyn_th+norm_th>1){ - updateAcrossMeasures(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); - updateAcrossMeasures(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); + if(dyn_th+ang_th+norm_th>0){ + obs_count += 1; + updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); + updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); } computeAngles(); PIDtick.loop();