Diff: run.cpp

Revision:

92:00460f6df439

Parent:

90:96c2b0ed4b96

Child:

93:b827f78a717a

--- a/run.cpp	Tue Oct 26 05:37:25 2021 +0000
+++ b/run.cpp	Thu Oct 28 09:44:47 2021 +0000
@@ -3,15 +3,11 @@
 void run()
 {
     wait(0.5);
-    Timer _t;
-    _t.start();
 
     //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    int n_init = 1;
     if(hilFlag == false){
         eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
     }
-    eskf.setGravity(0.0f,0.0f,-9.8f);
     
     //センサ正常性チェック
     //usaPack通信開始
@@ -21,18 +17,53 @@
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
+    Timer _t;
+    _t.start();
+    
+    float tgps = _t.read();
+    Matrix Rgpspos(3,3);
+    Rgpspos(1,1) = 5000.0f;
+    Rgpspos(2,2) = 5000.0f;
+    Rgpspos(3,3) = 5000.0f;
+    Matrix Rgpsvel(3,3);
+    Rgpsvel(1,1) = 5000.0f;
+    Rgpsvel(2,2) = 5000.0f;
+    Rgpsvel(3,3) = 5000.0f;
+    Matrix RaccConst(3,3);
+    RaccConst(1,1) = 980000.0f;
+    RaccConst(2,2) = 980000.0f;
+    RaccConst(3,3) = 980000.0f;
+    Matrix RgyroConst(2,2);
+    RgyroConst(1,1) = 100000.0f;
+    RgyroConst(2,2) = 100000.0f;
     while(1)
     {
         float tstart = _t.read();
         //センサの値を取得
         if(hilFlag == true){
-            getHILval();
+            getHilIMUval();
+            getIMUval();
         }else{
             //getIMUval();
         }
         //ekfの更新
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+        if(tstart-tgps>0.2f){
+            if(hilFlag == true){
+                getHilGPSval();
+            }else{
+                //getGPSval();
+            }
+            eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
+            eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
+            tgps = _t.read();
+        }else{
+            //eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst);
+            //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
+            eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
+            eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
+        }
         eskf.fuseErr2Nominal();
         
         PIDtick.loop();