HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

Diff: servo.cpp

Revision:
121:2523eef96b36
Parent:
109:eb255fc4462b
Child:
124:7d6b1b62483b
--- a/servo.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/servo.cpp	Fri Nov 19 07:56:16 2021 +0000
@@ -21,12 +21,16 @@
     rollPID.setProcessValue(rpy.x);
     rollratePID.setProcessValue(gyro.x);
     
-    if (true)
+    dT = rc[2];
+    
+    if (rc[6] > 0.0f)
     {
         //level_flight();
         //point_guide();
         turning();
         rollPID.setSetPoint(roll_obj);
+        pitchPID.setSetPoint(pitch_obj);
+        dT += dT_obj;
     }
     
     //舵角計算
@@ -37,8 +41,6 @@
         de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
         da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
     }
-
-    dT = rc[2];
     
     scaledServoOut[0]=de+da;
     scaledServoOut[1]=-de+da;