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:
- 90:96c2b0ed4b96
- Parent:
- 87:89bbbcdb667b
- Child:
- 92:00460f6df439
diff -r c9f64bd655d9 -r 96c2b0ed4b96 servo.cpp --- 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;