Diff: run.cpp

Revision:

126:03da3a8c8870

Parent:

125:5b5a91004b48

Child:

127:d73a6233ee4b

--- a/run.cpp	Fri Nov 19 14:24:29 2021 +0000
+++ b/run.cpp	Sun Nov 21 08:24:51 2021 +0000
@@ -5,10 +5,6 @@
     wait(0.5);
 
     //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    if(hilFlag == false){
-        eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
-    }
-    
     int n_init = 1000;
     for(int i = 0;i<n_init;i++){
         lsm.readAccel();
@@ -36,7 +32,7 @@
     //センサ正常性チェック
     //usaPack通信開始
     pc.Subscribe(0000, &(vp));
-    
+
     //制御ループのアタッチ
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
@@ -77,12 +73,9 @@
     Matrix Racc(3,3);
     setDiag(Racc,50.0f);
     
-    Matrix Rheading(2,2);
-    Rheading(1,1) = 50.0f;
-    Rheading(2,2) = 50.0f;
-    //Rheading(3,3) = 0.001f;
-    //Rheading(4,4) = 0.1f;
-        _t.start();
+    Matrix Rheading(1,1);
+    Rheading(1,1) = 0.1f;
+    _t.start();
     float tgps = _t.read();
     while(1)
     {
@@ -114,13 +107,7 @@
             //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
             
         }else{
-            //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
-            //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
-            //eskf.updateotherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst);
-            //if(_t.read()>20){
-                //eskf.updateMag(MatrixMath::Vector2mat(mag),Rheading);
-            //}
-            eskf.updateHeading(MatrixMath::Vector2mat(mag),Rheading);
+            eskf.updateHeading(-0.1f,Rheading);
             eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();