Diff: run.cpp

Revision:

129:a76be8380bb2

Parent:

127:d73a6233ee4b

Child:

134:d57c6b2a706b

--- a/run.cpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/run.cpp	Mon Nov 29 09:45:44 2021 +0000
@@ -29,13 +29,6 @@
     magref.z /= float(n_init);
     palt0 /= float(n_init);
     twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
-    //センサ正常性チェック
-    //usaPack通信開始
-    pc.Subscribe(0000, &(vp));
-
-    //制御ループのアタッチ
-    LoopTicker PIDtick;
-    PIDtick.attach(calcServoOut,PID_dt);
     
     //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
@@ -46,22 +39,84 @@
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
     
-    eskf.setQVelocity(0.000025f);
-    eskf.setQAngleError(0.00025f);
-    eskf.setQAccBias(0.0000001f);
+    eskf.setQVelocity(0.001f);
+    eskf.setQAngleError(0.0000025f);
+    eskf.setQAccBias(0.000001f);
     eskf.setQGyroBias(0.000001f);
     
+    Matrix Rgpspos(3,3);
+    setDiag(Rgpspos,1.0f);
+    
+    Matrix Rgpsvel(3,3);
+    Rgpsvel(1,1) = 0.01f;
+    Rgpsvel(2,2) = 0.01f;
+    Rgpsvel(3,3) = 0.1f;
+    
     Matrix Rgps(6,6);
-    setDiag(Rgps,1.0f);
-    //Rgps(4,4) = 0.1f;
-    //Rgps(5,5) = 0.1f;
+    setDiag(Rgps,0.05f);
+    Rgps(4,4) = 0.1f;
+    Rgps(5,5) = 0.1f;
     
+    float dynAccCriteria = 0.4f;
     Matrix Racc(3,3);
-    setDiag(Racc,50.0f);
+    setDiag(Racc,100.0f);
+    Matrix RaccDyn(3,3);
+    setDiag(RaccDyn,5000.0f);
 
     Matrix Rheading(1,1);
     Rheading(1,1) = 0.01f;
+    
     _t.start();
+    //センサ正常性チェック
+    if(hilFlag == false){
+        while(1){
+            float tstart = _t.read();
+            getIMUval();
+            getGPSval();
+            eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+            eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+            float heading = atan2f(-mag.y,mag.x);
+            eskf.updateHeading(heading,Rheading);
+            Matrix Raccpreflight(3,3);
+            setDiag(Raccpreflight,5.0f);
+            eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
+            
+            bool preflightCheck = true;
+            Matrix gyroBias = eskf.getGyroBias();
+            if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : high gyro value\r\n");
+            }
+            Matrix accBias = eskf.getAccBias();
+            if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : high acc value\r\n");
+            }
+            Matrix vihat = eskf.getVihat();
+            if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : high velocity value\r\n");
+            }
+            Matrix pihat = eskf.getPihat();
+            if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : not home position\r\n");
+            }
+            
+            if(preflightCheck == true){
+                break;
+            }
+        }
+    }
+    twelite.printf("PreFlight Check Completed\r\n");
+    //usaPack通信開始
+    pc.Subscribe(0000, &(vp));
+
+    //制御ループのアタッチ
+    LoopTicker PIDtick;
+    PIDtick.attach(calcServoOut,PID_dt);
+    
     float tgps = _t.read();
     float theading = _t.read();
     while(1)
@@ -86,6 +141,10 @@
                 gpsUpdateFlag = false;
             }
         }else{
+            if(userButton.read()==1)
+            {
+                gpsLlh0Fixed = false;
+            };  
             getGPSval();
         }
         
@@ -100,12 +159,20 @@
         
         if(gpsUpdateFlag == true){
             eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgps);
+            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
+            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
         }else if(headingUpdateFlag == true){
                 float heading = atan2f(-mag.y,mag.x);
                 eskf.updateHeading(heading,Rheading);
         }else{
-            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
+            dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
+            //twelite.printf("%f\r\n",sqrt(dynaccnorm2));
+            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
+                eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
+            }else{
+                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            }
         }
         PIDtick.loop();