Diff: servo.cpp

Revision:

90:96c2b0ed4b96

Parent:

87:89bbbcdb667b

Child:

92:00460f6df439

--- a/servo.cpp	Mon Oct 25 05:39:34 2021 +0000
+++ b/servo.cpp	Tue Oct 26 05:36:18 2021 +0000
@@ -7,27 +7,26 @@
     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
     }
-    
-    //rc[1] = joystick.getX();
-    //rc[0] = joystick.getY();
+
     
-    // 自身の位置に応じてエレベータ舵角を決定する
-    
-    //float rollObj = 20.0f*M_PI/180.0f*rc[0];
-    //float gaincoef = 1.0f;
-    //rollPID.setGain(5.0*gaincoef,0.0f,0.0);
-    //rollratePID.setGain(0.5*gaincoef,0.0f,0.0);
+    //姿勢角の所得
+    Matrix euler = eskf.computeAngles();
+    rpy.x = euler(1,1);
+    rpy.y = euler(2,1);
+    rpy.z = euler(3,1);
+
+    //PIDへの状態量のセット
     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];
+    //da = rc[0];
     de = -(pitchPID.compute()+pitchratePID.compute())+rc[1];
     da = (rollPID.compute()+rollratePID.compute())+rc[0];
-    //float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
-    //float da = rollPID.compute()+rollratePID.compute()+rc[0];
+
     dT = rc[2];
     
     scaledServoOut[0]=-de+da;