Diff: servo.cpp

Revision:

77:2bf856e3eca4

Parent:

76:7fd3ac1afe3e

Child:

82:c183c29d2427

--- a/servo.cpp	Sat Aug 07 06:54:58 2021 +0000
+++ b/servo.cpp	Tue Sep 07 08:13:45 2021 +0000
@@ -19,14 +19,19 @@
     
     // 自身の位置に応じてエレベータ舵角を決定する
     
+    float rollObj = 20.0f*M_PI/180.0f*rc[0];
+    float gaincoef = (rc[5]+1.0f)/2.0f*1.9f+1.0f;
+    rollPID.setGain(5.0*gaincoef,0.0f,0.0);
+    rollratePID.setGain(0.5*gaincoef,0.0f,0.0);
     pitchPID.setProcessValue(rpy.x);
     pitchratePID.setProcessValue(gyro.x);
     rollPID.setProcessValue(rpy.y);
+    rollPID.setSetPoint(rollObj);
     rollratePID.setProcessValue(gyro.y);
     //de = -rc[1];
     //da = +rc[0];
     de = -(pitchPID.compute()+pitchratePID.compute()-rc[1]);
-    da = rollPID.compute()+rollratePID.compute()+rc[0];
+    da = rollPID.compute()+rollratePID.compute();
     //float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
     //float da = rollPID.compute()+rollratePID.compute()+rc[0];
     dT = rc[2];