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
diff -r a0da2ce20a8e -r 2523eef96b36 servo.cpp --- 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;