Diff: servo.cpp

Revision:

57:923e3df16159

Parent:

56:888379912f81

Child:

61:c05353850017

--- a/servo.cpp	Mon May 31 18:59:36 2021 +0000
+++ b/servo.cpp	Tue Jun 01 07:30:06 2021 +0000
@@ -11,11 +11,37 @@
             rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
         }
     }
-    pitchPID.setGain(6.36, 10.6,0.0);
-    pitchratePID.setGain(0.9540,0.0,0.0);
+    
+    // 自身の位置に応じてエレベータ舵角を決定する
+    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;
+    }
+    //指令角の最大最小値をrc7で変更
+    float objgain = ((rc[6]+1.0f)/2*10.0f+1.0f)*M_PI/180.0f;
+    float pitchobj = objgain * deobj;
+    
+    //ゲインの係数をrc8で変更
+    float gaincoef = ((rc[7]+1.0f)/2*0.9)+0.1;
+    pitchPID.setGain(6.36*gaincoef, 10.6*gaincoef,0.0);
+    pitchratePID.setGain(0.9540*gaincoef,0.0,0.0);
     pitchPID.setProcessValue(rpy.y);
     pitchratePID.setProcessValue(gyro.y);
-    float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
+    //目標値のセット
+    pitchPID.setSetPoint(pitchobj); //目標の設定
+    float de = pitchPID.compute()+pitchratePID.compute();
     
     scaledServoOut[0]=de;