Diff: servo.cpp

Revision:

66:e5afad70fdd8

Parent:

62:ef10fd919f7b

Child:

68:b9f6938fab9d

--- a/servo.cpp	Tue Jun 22 02:08:31 2021 +0000
+++ b/servo.cpp	Tue Jun 22 02:19:14 2021 +0000
@@ -3,36 +3,13 @@
 // 割り込まれた時点での出力(computeの結果）を返す関数
 void calcServoOut()
 {
-    if(sbus.failSafe == false)
-    {
-        // sbusデータの読み込み
-        for (int i = 0; i < 16;i ++)
-        {
-            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
-        }
-    }
-    //pc.printf("rc:%f\r\n",rc[1]);
     
     // 自身の位置に応じてエレベータ舵角を決定する
     float deobj = 0.0f;
-    switch(pos_tail)
-    {
-    case 0: //left
-        deobj = -rc[1];
-        break;
-    case 1: //center
-        deobj = -rc[1];
-        break;
-    case 2: //right
-        deobj = -rc[1];
-        break;
-    default:   // error situation
-        deobj = 0.0f;
-        break;
-    }
+    float drobj = 0.0f;
+    
     //指令角の最大最小値をrc7で変更
-    float objgain = ((rc[6]+1.0f)/2*10.0f+1.0f)*M_PI/180.0f;
-    float pitchobj = objgain * deobj;
+    float pitchobj = 10.0f*M_PI/180.0f * deobj;
     
     //ゲインの係数をrc8で変更
     float gaincoef = 1.0;
@@ -43,9 +20,9 @@
     //目標値のセット
     pitchPID.setSetPoint(pitchobj); //目標の設定
     float de = pitchPID.compute()+pitchratePID.compute();
-    
+    float dr = 0.0f;
     scaledServoOut[0]=de;
-    
+    scaledServoOut[1]=dr;
     float LP_servo = 0.2;
     for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
     {
@@ -60,34 +37,13 @@
         }
     }
     
-    servo.pulsewidth_us(servoOut[0]);
-     
-    //観測アップデート
-    dynacc = ekf.calcDynAcc(LPacc, accref);
-    th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg);
-    
-    accnormerr = abs(LPacc.Norm()-accref.Norm());
-    //静止時100個の平均　0.01877522 0.00514146  0.00477393
-
-    //int ang_th  = th_mg < 0.01877522;
-    //int dyn_th = dynaccnorm < 0.00514146;
-    //int norm_th =  accnormerr< 0.00477393;
-    int ang_th  = th_mg < sigma_thmg/200.0;
-    int dyn_th = dynacc.Norm() < sigma_accnorm/50.0;
-    int norm_th =  accnormerr< sigma_accnorm/50.0;
-    if(dyn_th+ang_th+norm_th>0)
-    {
-    //if(dyn_th+ang_th+norm_th>-1){
-            obs_count += 1;
-            ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
-            ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
-    } 
+    elevServo.pulsewidth_us(servoOut[0]);
+    rudServo.pulsewidth_us(servoOut[1]);
      
     if(loop_count >= 10)
     {
         writeSdcard();
         loop_count = 1;
-        obs_count = 0;
     }
     else
     {