Diff: run.cpp

Revision:

87:89bbbcdb667b

Parent:

86:456f00d52974

Child:

88:be349faa1976

--- a/run.cpp	Mon Oct 18 12:18:15 2021 +0000
+++ b/run.cpp	Wed Oct 20 01:50:52 2021 +0000
@@ -2,50 +2,41 @@
 
 void run()
 {
-    //pc.serial.printf("\r\nrun Mode\r\n");
     wait(0.5);
     Timer _t;
     _t.start();
-    magCalibrator.setExtremes(magbiasMin,magbiasMax);
 
-    float sum2accnorm = 0;
+    //センサの初期化・ジャイロバイアス・加速度スケールの取得
     float sumaccnorm = 0;
     int n_init = 1000;
     if(hilFlag == false){
         ekf.defineQhat(rpy_align);
         for(int i = 0; i < n_init; i++){
-            float tstart = _t.read();
-            getIMUval();
-            ekf.updateStaticAccMeasures(acc,accref);
-            //ekf.updateGyroBiasConstraints(gyro);
-            ekf.fuseErr2Nominal();
-            ekf.resetBias();
-            //ekf.updateMagMeasures(mag);
-            ekf.computeAngles(rpy, rpy_align);
             sumaccnorm += acc.Norm();
-            sum2accnorm += acc.Norm()*acc.Norm();
-            float tend = _t.read();
-            att_dt = (tend-tstart);
         }
         accref.z =  sumaccnorm / float(n_init);
     }
+    
+    //センサ正常性チェック
+
+    //usaPack通信開始
     pc.Subscribe(0000, &(vp));
     
+    //制御ループのアタッチ
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
-
-    
     while(1)
     {
         float tstart = _t.read();
-        //姿勢角を更新
+        //センサの値を取得
         if(hilFlag == true){
             getHILval();
         }else{
-            getIMUval();
+            //getIMUval();
         }
 
+        //ekfの更新
         ekf.updateNominal(gyro,acc,accref,att_dt);
         ekf.updateErrState(gyro,acc, att_dt);
         if(obsCount == 50){
@@ -70,55 +61,11 @@
         ekf.fuseErr2Nominal();
         ekf.resetBias();
         ekf.computeAngles(rpy, rpy_align);
-        ekf.computeVb(vb);
+        
         PIDtick.loop(); 
         
+        //制御時間を計測
         float tend = _t.read();
         att_dt = (tend-tstart);
     }
-}
-
-    /*
-    if(serialParamSource){
-        while(1){
-            pc.serial.attach(NULL, Serial::RxIrq);
-            pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]);
-            pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq);
-            switch(vp.commandIndex){
-            case 1:
-                NVIC_SystemReset();
-                break;
-            case 10:
-                ekf.setQqerr(float(vp.commandVal));
-                checkParamSerial[0] = 1;
-                break;
-            case 11:
-                ekf.setQgbias(float(vp.commandVal));
-                checkParamSerial[1] = 1;
-                break;
-            case 12:
-                Qabdyn=float(vp.commandVal);
-                checkParamSerial[2] = 1;
-                break;
-            case 13:
-                Rscdyn=float(vp.commandVal);
-                checkParamSerial[3] = 1;
-                break;
-            case 14:
-                
-                checkParamSerial[4] = 1;
-                break;
-            default :
-                break;
-            }
-            int cpsSum = 0;
-            for(int i = 0;i<5;i++){
-                cpsSum +=  checkParamSerial[i];
-            }
-            if(cpsSum == 5){
-                break;
-            }
-            wait(0.01);
-        }   
-    }
-    */
\ No newline at end of file
+}
\ No newline at end of file