Diff: main.cpp

Revision:

44:f571273d3223

Parent:

43:f0dd73430192

Child:

45:b439a135c24b

Child:

46:99ffb98e1257

--- a/main.cpp	Fri Apr 02 07:22:09 2021 +0000
+++ b/main.cpp	Thu Apr 08 07:37:23 2021 +0000
@@ -43,6 +43,7 @@
 Timer t;
 
 Matrix qhat(4,1);
+Matrix qhat_gyro(4,1);
 Matrix Phat(4,4);
 Matrix Qgyro(3,3);
 Matrix Racc(3,3);
@@ -56,6 +57,11 @@
 float pitch = 0.0;
 float roll = 0.0;
 float yaw = 0.0;
+
+float pitch_g = 0.0;
+float roll_g = 0.0;
+float yaw_g = 0.0;
+
 int16_t ax, ay, az;
 int16_t gx, gy, gz;
 MotionSensorDataUnits mdata;
@@ -93,21 +99,22 @@
 float accref[3] = {0, 0, -0.98};
 float magref[3] = {0.65, 0, 0.75};
  
-int agoffset[6] = {0, 0, 0, -123, -575, -1};
-float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077};
+int agoffset[6] = {0, 0, 0, -117, -563, 2 };
+float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610};
 
 void writeSdcard()
 {
     //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]);
-    //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);
+    sd.printf("%f %d %f %f %f %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,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+    //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*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);
     //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
-    //pc.printf("%f %f %f %f %f : %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+    //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z);
     //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
 }
 
@@ -117,17 +124,13 @@
 }
 
 void calcMagRef(float mx, float my, float mz){
-    float q0 = qhat.getNumber( 1, 1 );
-    float q1 = qhat.getNumber( 2, 1 ); 
-    float q2 = qhat.getNumber( 3, 1 ); 
-    float q3 = qhat.getNumber( 4, 1 ); 
-    float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2));
-    float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1));
-    float bx = sqrt(hx * hx + hy * hy);
-    float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2));
-    magref[0] = bx;
+    
+    Matrix magvec(3,1);
+    magvec << mx << my << mz;
+    Matrix magnedvec = MatrixMath::Transpose(D)*magvec;
+    magref[0] = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
     magref[1] = 0.0;
-    magref[2] = bz;
+    magref[2] = magnedvec(3,1);
 }
 
 
@@ -195,11 +198,15 @@
         <<  gyro_x*0.5*att_dt << 1.0                << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
         <<  gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0               << gyro_x*0.5*att_dt
         <<  gyro_z*0.5*att_dt <<  gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
+    
     qhat = phi*qhat;
-    
     float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
     qhat *= (1.0f/ qnorm);
     
+    qhat_gyro = phi*qhat_gyro;
+    qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro));
+    qhat_gyro *= (1.0f/ qnorm);
+    
     Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
     
     q0 = qhat.getNumber( 1, 1 );
@@ -295,6 +302,15 @@
     roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
     pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
     yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    
+    q0 = qhat_gyro.getNumber( 1, 1 );
+    q1 = qhat_gyro.getNumber( 2, 1 ); 
+    q2 = qhat_gyro.getNumber( 3, 1 ); 
+    q3 = qhat_gyro.getNumber( 4, 1 ); 
+    roll_g = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
+    pitch_g = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
+    yaw_g = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    
 }
 
 void triad(float fb1,float fb2, float fb3, float fn1, float fn2, float fn3,float mb1,float mb2, float mb3, float mn1, float mn2, float mn3){
@@ -342,6 +358,24 @@
    
     float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
     qhat *= (1.0f/ qnorm);
+    
+    qhat_gyro = qhat;
+    
+    float q0 = qhat.getNumber( 1, 1 );
+    float q1 = qhat.getNumber( 2, 1 ); 
+    float q2 = qhat.getNumber( 3, 1 ); 
+    float 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);
+    
 }
 
 void calcDynAcc(){
@@ -472,13 +506,35 @@
     servoLeft.pulsewidth_us(servoOut[1]); 
     servoThrust.pulsewidth_us(motorOut[0]);
      
-    if(loop_count > 10){
+    //観測アップデート
+    calcDynAcc();
+    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);
+    //静止時100個の平均　0.01877522 0.00514146  0.00477393
+
+    //int ang_th  = th_mg < 0.01877522;
+    //int dyn_th = dynaccnorm < 0.00514146;
+    //int norm_th =  accnormerr< 0.00477393;
+    int ang_th  = th_mg < 0.01877522/5.0;
+    int dyn_th = dynaccnorm < 0.00514146/5.0;
+    int norm_th =  accnormerr< 0.00477393/5.0;
+    if(dyn_th+ang_th+norm_th>0){
+    //if(dyn_th+ang_th+norm_th>-1){
+            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);
+    } 
+     
+    if(loop_count >= 10){
         writeSdcard();
         loop_count = 1;
         obs_count = 0;
     }else{
         loop_count +=1;
     }
+    
+
 }
 
 int main()
@@ -522,23 +578,28 @@
     
     if(userButton.read() == 0){
         qhat << 1 << 0 << 0 << 0;
+        
+        D.add(1,1,1.0);
+        D.add(2,2,1.0);
+        D.add(3,3,1.0);
     
         Phat << 0.01 << 0 <<0 <<0
              << 0 << 0.01 <<0 <<0
              << 0 << 0 <<0.001 <<0
              << 0 << 0 << 0 << 0.001;
         
-        Qgyro << 5.4732e-04 << 0     <<0
-              << 0     <<5.4732e-04 <<0
-              << 0     << 0     <<5.4732e-04;  
+        Qgyro << 0.0224<< 0     <<0
+              << 0     <<0.0224<<0
+              << 0     << 0     <<0.0224;  
         
-        Racc.add(1,1,1.0);
-        Racc.add(2,2,1.0);
-        Racc.add(3,3,1.0);
-             
-        Rmag << 1   << 0   <<0
-             << 0   << 1   <<0
-             << 0   << 0   <<1;   
+        Racc.add(1,1,0.0330*100);
+        Racc.add(2,2,0.0330*100);
+        Racc.add(3,3,0.0330*100);
+        
+        Rmag.add(1,1,1.0);
+        Rmag.add(2,2,1.0);
+        Rmag.add(3,3,1.0); 
+  
         getIMUval();
         triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm);
         float sumLPaccnorm = 0;
@@ -559,20 +620,6 @@
             //姿勢角を更新
             getIMUval();
             updateBetweenMeasures();
-            calcDynAcc();
-            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  = 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(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();