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:
- 77:2bf856e3eca4
- Parent:
- 76:7fd3ac1afe3e
- Child:
- 82:c183c29d2427
diff -r 7fd3ac1afe3e -r 2bf856e3eca4 servo.cpp --- 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];