Diff: run.cpp

Revision:

73:84ffa0166e6c

Parent:

70:99f974d8960e

Child:

74:f67062e7813e

--- a/run.cpp	Wed Jun 30 01:16:21 2021 +0000
+++ b/run.cpp	Thu Jul 15 05:26:15 2021 +0000
@@ -7,22 +7,22 @@
     Timer _t;
     _t.start();
     magCalibrator.setExtremes(magbiasMin,magbiasMax);
-    for(int i = 0; i < 1000; i++){
-        getIMUval();
-    }
-    //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
     ekf.defineQhat(rpy_align);
     float sum2accnorm = 0;
     float sumaccnorm = 0;
     for(int i = 0; i < 1000; i++){
         float tstart = _t.read();
-        getIMUval();
-        ekf.updateQhat(gyro, att_dt);
-        ekf.updateErrState(gyro, att_dt);
+        if(hilFlag == true){
+            getHILval();
+        }else{
+            getIMUval();
+        }
+        //ekf.updateQhat(gyro, att_dt);
+        //ekf.updateErrState(gyro, att_dt);
         ekf.updateStaticAccMeasures(acc,accref);
-        ekf.fuseErr2Qhat();
-        //ekf.resetGyroBias(gyroBias);
-        ekf.updateMagMeasures(mag);
+        //ekf.fuseErr2Qhat();
+        //ekf.resetBias();
+        //ekf.updateMagMeasures(mag);
         ekf.computeAngles(rpy, rpy_align);
         sumaccnorm += acc.Norm();
         sum2accnorm += acc.Norm()*acc.Norm();
@@ -32,7 +32,7 @@
     accref.z =  sumaccnorm / 1000.0f;
     float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
     pc.serial.printf("%f \r\n",sigma_accnorm);
-    pc.Subscribe(0000, &(posValues));
+    pc.Subscribe(0000, &(vp));
     
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
@@ -43,7 +43,11 @@
     {
         float tstart = _t.read();
         //姿勢角を更新
-        getIMUval();
+        if(hilFlag == true){
+            getHILval();
+        }else{
+            getIMUval();
+        }
         ekf.updateQhat(gyro, att_dt);
         ekf.updateErrState(gyro, att_dt);
         if(ekf.determinDynStatus(acc,accref)){
@@ -53,7 +57,8 @@
         }
         ekf.fuseErr2Qhat();
         ekf.resetBias();
-        ekf.updateMagMeasures(mag);
+        //ekf.updateMagMeasures(mag);
+        //ekf.gyroBiasEstimation(mag,gyro,att_dt);
         ekf.computeAngles(rpy, rpy_align);
         PIDtick.loop();