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();