Diff: run.cpp

Revision:

90:96c2b0ed4b96

Parent:

89:c9f64bd655d9

Child:

92:00460f6df439

--- a/run.cpp	Mon Oct 25 05:39:34 2021 +0000
+++ b/run.cpp	Tue Oct 26 05:36:18 2021 +0000
@@ -7,18 +7,13 @@
     _t.start();
 
     //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    float sumaccnorm = 0;
     int n_init = 1;
     if(hilFlag == false){
-        ekf.defineQhat(rpy_align);
-        for(int i = 0; i < n_init; i++){
-            sumaccnorm += acc.Norm();
-        }
-        accref.z =  sumaccnorm / float(n_init);
+        eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
     }
+    eskf.setGravity(0.0f,0.0f,-9.8f);
     
     //センサ正常性チェック
-
     //usaPack通信開始
     pc.Subscribe(0000, &(vp));
     
@@ -32,36 +27,13 @@
         //センサの値を取得
         if(hilFlag == true){
             getHILval();
-            //getIMUval();
         }else{
             //getIMUval();
         }
-
         //ekfの更新
-        ekf.updateNominal(gyro,acc,accref,att_dt);
-        ekf.updateErrState(gyro,acc, att_dt);
-        if(obsCount == 50){
-            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
-            //ekf.updateSinkRate(vi.z,acc,accref);
-            obsCount = 0;
-        }else{
-            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
-            //ekf.updateAccMeasures(acc,accref);
-            ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
-            if(ekf.determinDynStatus(acc,accref)){
-                //ekf.updateAccMeasures(acc,accref);
-            }else{   
-                //ekf.updateAccMeasures(acc,accref);
-            }
-            
-            obsCount += 1;
-        }
-        //ekf.updateSinkRate(vi.z,acc,accref);
-        //ekf.updateGyroBiasConstraints(gyro);
-        
-        ekf.fuseErr2Nominal();
-        ekf.resetBias();
-        ekf.computeAngles(rpy, rpy_align);
+        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+        eskf.fuseErr2Nominal();
         
         PIDtick.loop();