Diff: run.cpp

Revision:

143:53808e4e684c

Parent:

141:725321fe2949

--- a/run.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/run.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -2,137 +2,40 @@
 
 void run()
 {
-    wait(0.5);
-
-    //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    int n_init = 1000;
-    for(int i = 0;i<n_init;i++){
-        lsm.readAccel();
-        lsm.readMag();
-        lsm.readGyro();
-        agoffset[0] += lsm.ax * 9.8f;
-        agoffset[1] += lsm.ay * 9.8f;
-        agoffset[2] += lsm.az * 9.8f-9.8f;
-        agoffset[3] +=(lsm.gx * M_PI_F / 180.0f);
-        agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); 
-        agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); 
-        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
-        magref(0) += lsm.mx;
-        magref(1) += lsm.my;
-        magref(2) += lsm.mz;
-    }
-    for(int i = 0;i<6;i++){
-        agoffset[i] /= float(n_init);
-    }
-    magref /= float(n_init);
-    palt0 /= float(n_init);
-    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
+    preflightCalibration();
     
-    //ESKFの共分散設定
-    eskf.setGravity(0.0f,0.0f,9.8f);
-    eskf.setPhatPosition(0.1f);
-    eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(0.5f);
-    eskf.setPhatAccBias(0.001f);
-    eskf.setPhatGyroBias(0.001f);
-    eskf.setPhatGravity(0.0000001f);
-    
-    eskf.setQVelocity(0.001f);
-    eskf.setQAngleError(0.0000025f);
-    eskf.setQAccBias(0.000001f);
-    eskf.setQGyroBias(0.000001f);
-    
-    Matrix3f Rgpspos;
-    setDiag(Rgpspos,1.0f);
-    
-    Matrix3f Rgpsvel;
-    Rgpsvel(0,0) = 0.01f;
-    Rgpsvel(1,1) = 0.01f;
-    Rgpsvel(2,2) = 0.1f;
+    setEskfCov();
     
     MatrixXf Rgps(5,5);
-    setDiag(Rgps,0.05f);
+    setDiag(Rgps,1.5f);
+    Rgps(2,2) = 0.1f;
     Rgps(3,3) = 0.1f;
     Rgps(4,4) = 0.1f;
     
-    float dynAccCriteria = 0.4f;
-    Matrix3f Racc;
-    setDiag(Racc,100.0f);
-    Matrix3f RaccDyn;
-    setDiag(RaccDyn,5000.0f);
-
-    Matrix<float, 1, 1> Rheading;
-    Rheading(0, 0) = 0.01f;
+    MatrixXf Rwhole = MatrixXf::Zero(9,9);
+    Rwhole(0,0) = 1.5f;
+    Rwhole(1,1) = 1.5f;
+    Rwhole(2,2) = 0.1f;
+    Rwhole(3,3) = 0.1f;
+    Rwhole(4,4) = 0.1f;
+    Rwhole(5,5) = 5000.0f;
+    Rwhole(6,6) = 5000.0f;
+    Rwhole(7,7) = 5000.0f;
+    Rwhole(8,8) = 0.007f;
+    
     
     _t.start();
-    //センサ正常性チェック
-    if(hilFlag == false){
-        while(1){
-            float tstart = _t.read();
-            getIMUval();
-            getGPSval();
-            eskf.updateNominal(acc, gyro, att_dt);
-            eskf.updateErrState(acc, gyro, att_dt);
-            eskf.updateGPS(pi, palt, vi, Rgps);
-            float heading = std::atan2(-mag(2), mag(0));
-            eskf.updateHeading(heading, Rheading);
-            Matrix3f Raccpreflight;
-            setDiag(Raccpreflight, 5.0f);
-            eskf.updateAcc(acc, Raccpreflight);
-            
-            bool preflightCheck = true;
-            Vector3f gyroBias = eskf.getGyroBias();
-            if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : high gyro value\r\n");
-            }
-            Vector3f accBias = eskf.getAccBias();
-            if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : high acc value\r\n");
-            }
-            Vector3f vihat = eskf.getVihat();
-            if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : high velocity value\r\n");
-            }
-            Vector3f pihat = eskf.getPihat();
-            if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>2.0f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : not home position\r\n");
-            }
-            if(sbus.failSafe){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no RC\r\n");
-            }
-            // sbusデータの読み込み
-            for (int i =0 ; i < 16;i ++){
-                rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
-            }
-            if (rc[4]>-0.3f && rc[6] < -0.3f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
-            }
-            if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
-            }
-            
-            if(preflightCheck == true){
-                break;
-            }
-        }
-    }
-    twelite.serial.printf("PreFlight Check Completed\r\n");
-    //usaPack通信開始
+
+    preflightCheck();
+    wait(1.0f);
+    //usaPack通信開始 制御ループのアタッチ
     pc.Subscribe(0000, &(vp));
-
-    //制御ループのアタッチ
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
     float tgps = _t.read();
     float theading = _t.read();
+    float tstart = _t.read();
     while(1)
     {
         float tstart = _t.read();
@@ -162,32 +65,22 @@
             getGPSval();
         }
         
-        headingUpdateFlag = false;
-        if(tstart-theading>0.05f){
-            Vector3f euler = eskf.computeAngles();
-            if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){
-                headingUpdateFlag = true;
-                theading = _t.read();
+        if(gpsUpdateFlag == true){
+            float heading = std::atan2(-mag(1),mag(0));
+            Vector3f dynacc = eskf.calcDynAcc(acc);
+            dynaccnorm2 = dynacc.squaredNorm();
+            if(dynaccnorm2 > 0.16f){
+                Rwhole(5,5) = 100.0f;
+                Rwhole(6,6) = 100.0f;
+                Rwhole(7,7) = 100.0f;
+            }else{
+                Rwhole(5,5) = 5000.0f;
+                Rwhole(6,6) = 5000.0f;
+                Rwhole(7,7) = 5000.0f;
             }
+            eskf.updateWhole(pi, palt, vi,acc,heading, Rwhole); 
         }
         
-        if(gpsUpdateFlag == true){
-            eskf.updateGPS(pi, palt, vi, Rgps);
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
-            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
-        }else if(headingUpdateFlag == true){
-                float heading = std::atan2(-mag(1),mag(0));
-                eskf.updateHeading(heading,Rheading);
-        }else{
-            Vector3f dynacc = eskf.calcDynAcc(acc);
-            dynaccnorm2 = dynacc.squaredNorm();
-            //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
-            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
-                eskf.updateAcc(acc, RaccDyn);
-            }else{
-                eskf.updateAcc(acc, Racc);
-            }
-        }
         PIDtick.loop(); 
         
         //制御時間を計測