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
servo.cpp
- Committer:
- NaotoMorita
- Date:
- 22 months ago
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
File content as of revision 143:53808e4e684c:
#include "global.hpp" // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut() { // sbusデータの読み込み for (int i =0 ; i < 16;i ++){ rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input } //姿勢角の所得 euler = eskf.computeAngles(); Vector3f vihat = eskf.getVihat(); Vector3f gyroBias = eskf.getGyroBias(); //PIDへの状態量のセット 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(); 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{ 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; } scaledServoOut[0]=-de-da; scaledServoOut[1]=de-da; scaledMotorOut[0]= dT; 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,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i]; if(servoOut[i]<servoPwmMin) { servoOut[i] = servoPwmMin; } if(servoOut[i]>servoPwmMax) { servoOut[i] = servoPwmMax; } } for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;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; }; if(motorOut[i]>motorPwmMax) { motorOut[i] = motorPwmMax; }; } servoRight.pulsewidth_us(servoOut[0]); servoLeft.pulsewidth_us(servoOut[1]); servoThrust.pulsewidth_us(motorOut[0]); sendData2PC(); writeSDcard(); if(loop_count >= 5) { sendTelemetry(); loop_count = 1; } else { loop_count +=1; } }