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:
- 123:b63d3524ffbc
- Parent:
- 109:eb255fc4462b
--- a/servo.cpp Thu Nov 18 10:10:18 2021 +0000 +++ b/servo.cpp Fri Nov 19 08:22:13 2021 +0000 @@ -21,12 +21,16 @@ rollPID.setProcessValue(rpy.x); rollratePID.setProcessValue(gyro.x); - if (true) + dT = rc[2]; + + if (rc[4]>-0.3f&&rc[6] > 0.0f) { //level_flight(); //point_guide(); turning(); rollPID.setSetPoint(roll_obj); + pitchPID.setSetPoint(pitch_obj); + dT += dT_obj; } //舵角計算 @@ -38,7 +42,6 @@ da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f; } - dT = rc[2]; scaledServoOut[0]=de+da; scaledServoOut[1]=-de+da;