solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: servo.cpp
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
- Child:
- 144:b3a713b4f1c4
--- a/servo.cpp Fri Dec 10 11:20:13 2021 +0000 +++ b/servo.cpp Fri Jun 24 05:44:34 2022 +0000 @@ -11,47 +11,93 @@ //姿勢角の所得 euler = eskf.computeAngles(); - rpy = euler; - + + Vector3f vihat = eskf.getVihat(); + Vector3f gyroBias = eskf.getGyroBias(); //PIDへの状態量のセット - pitchPID.setProcessValue(rpy(1)); - pitchratePID.setProcessValue(gyro(1)); - rollPID.setProcessValue(rpy(0)); - rollratePID.setProcessValue(gyro(0)); + pitchPID.setProcessValue(euler(1)); + pitchratePID.setProcessValue(gyro(1)-gyroBias(1)); + rollPID.setProcessValue(euler(0)); + rollratePID.setProcessValue(gyro(0)-gyroBias(0)); + yawratePID.setProcessValue(gyro(2)-gyroBias(2)); + climbratePID.setProcessValue(vihat(2)); dT = rc[2]; + // ゲイン切り替え + float vihat_norm = vihat.squaredNorm(); + if (rc[5] < 0.3f){ + // Hinf + hinf_flag = true; + if (vihat_norm < 9.0f || rc[2] < -0.8f){ + pitchPID.setGain(4.55f, 0.0f, 0.0f); + pitchratePID.setGain(0.363f, 0.0f, 0.0f); + rollPID.setGain(1.34f, 0.0f, 0.0f); + rollratePID.setGain(0.219f, 0.0f,0.0f); + pitchPID.resetIntError(); + rollPID.resetIntError(); + }else{ + pitchPID.setGain(4.55f, 9.7f, 0.0f); + pitchratePID.setGain(0.363f, 0.0f, 0.0f); + rollPID.setGain(1.34f, 0.007f, 0.0f); + rollratePID.setGain(0.219f, 0.0f,0.0f); + } + }else{ + // 従来 + hinf_flag = false; + if (vihat_norm < 9.0f || rc[2] < -0.8f){ + pitchPID.setGain(10.0f, 0.0f, 0.0f); + pitchratePID.setGain(1.0f, 0.0f, 0.0f);//rad/s + rollPID.setGain(5.0f, 0.0f, 0.0f); + rollratePID.setGain(0.05f, 0.0f, 0.0f);//rad/s + pitchPID.resetIntError(); + rollPID.resetIntError(); + }else{ + pitchPID.setGain(10.0f, 0.0f, 0.0f); + pitchratePID.setGain(1.0f, 0.0f, 0.0f);//rad/s + rollPID.setGain(5.0f, 0.0f, 0.0f); + rollratePID.setGain(0.05f, 0.0f, 0.0f);//rad/s + } + } + if (rc[4]>-0.3f && rc[6] < -0.3f) { //level_flight(); //point_guide(); - climb(); + //climb(); + + turning(); rollPID.setSetPoint(roll_obj); pitchPID.setSetPoint(pitch_obj); dT += dT_obj; + de = pitchPID.compute()-(rc[0]-rc[1])/2.0f; + da = rollPID.compute() -(rc[0]+rc[1])/2.0f; }else{ - rollPID.setSetPoint(0.0f); - pitchPID.setSetPoint(0.0f); + yawratePID.setSetPoint(0.0f); + roll_obj = yawratePID.compute()*30.0f*M_PI/180.0f; + rollPID.setSetPoint(roll_obj); + climbratePID.setSetPoint(0.0f); + pitch_obj = -climbratePID.compute()*10.0f*M_PI/180.0f; + pitchPID.setSetPoint(pitch_obj); + de = pitchPID.compute() -(rc[0]-rc[1])/2.0f*2.0f; + da = rollPID.compute() -(rc[0]+rc[1])/2.0f*3.0f; } //舵角計算 if(rc[4]<-0.3f){ - de = (rc[0]-rc[1])/2.0f; - da = (rc[0]+rc[1])/2.0f; - }else{ - de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f; - da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f; + de = -(rc[0]-rc[1])/2.0f; + da = -(rc[0]+rc[1])/2.0f; } - scaledServoOut[0]=de+da; - scaledServoOut[1]=-de+da; + scaledServoOut[0]=-de-da; + scaledServoOut[1]=de-da; scaledMotorOut[0]= dT; - float LP_servo = 0.2; - float LP_motor = 0.2; + float LP_servo = 1.0f; + float LP_motor = 0.2f; for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++) { - servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1.0f,1.0f,servoPwmMin,servoPwmMax))+(1.0f-LP_servo)*servoOut[i]; + servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i]; if(servoOut[i]<servoPwmMin) { servoOut[i] = servoPwmMin; @@ -63,7 +109,7 @@ } for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){ - motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax))+(1.0f-LP_motor)*motorOut[i]; + motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i]; if(motorOut[i]<motorPwmMin) { motorOut[i] = motorPwmMin; }; @@ -75,6 +121,7 @@ servoLeft.pulsewidth_us(servoOut[1]); servoThrust.pulsewidth_us(motorOut[0]); + sendData2PC(); writeSDcard();