HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@143:53808e4e684c, 23 months ago (annotated)

Committer:
NaotoMorita
Date:
Fri Jun 24 05:44:34 2022 +0000
Revision:
143:53808e4e684c
Parent:
141:725321fe2949 
complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 140:53dbdb207542 1 #include "global.hpp"
cocorlow 140:53dbdb207542 2
cocorlow 140:53dbdb207542 3 // 割り込まれた時点での出力(computeの結果）を返す関数
cocorlow 140:53dbdb207542 4 void calcServoOut()
cocorlow 140:53dbdb207542 5 {
cocorlow 140:53dbdb207542 6 // sbusデータの読み込み
cocorlow 140:53dbdb207542 7 for (int i =0 ; i < 16;i ++){
cocorlow 141:725321fe2949 8 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
cocorlow 140:53dbdb207542 9 }
cocorlow 140:53dbdb207542 10
cocorlow 140:53dbdb207542 11
cocorlow 140:53dbdb207542 12 //姿勢角の所得
cocorlow 140:53dbdb207542 13 euler = eskf.computeAngles();
NaotoMorita 143:53808e4e684c 14
NaotoMorita 143:53808e4e684c 15 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 16 Vector3f gyroBias = eskf.getGyroBias();
cocorlow 140:53dbdb207542 17 //PIDへの状態量のセット
NaotoMorita 143:53808e4e684c 18 pitchPID.setProcessValue(euler(1));
NaotoMorita 143:53808e4e684c 19 pitchratePID.setProcessValue(gyro(1)-gyroBias(1));
NaotoMorita 143:53808e4e684c 20 rollPID.setProcessValue(euler(0));
NaotoMorita 143:53808e4e684c 21 rollratePID.setProcessValue(gyro(0)-gyroBias(0));
NaotoMorita 143:53808e4e684c 22 yawratePID.setProcessValue(gyro(2)-gyroBias(2));
NaotoMorita 143:53808e4e684c 23 climbratePID.setProcessValue(vihat(2));
cocorlow 140:53dbdb207542 24
cocorlow 140:53dbdb207542 25 dT = rc[2];
cocorlow 140:53dbdb207542 26
NaotoMorita 143:53808e4e684c 27 // ゲイン切り替え
NaotoMorita 143:53808e4e684c 28 float vihat_norm = vihat.squaredNorm();
NaotoMorita 143:53808e4e684c 29 if (rc[5] < 0.3f){
NaotoMorita 143:53808e4e684c 30 // Hinf
NaotoMorita 143:53808e4e684c 31 hinf_flag = true;
NaotoMorita 143:53808e4e684c 32 if (vihat_norm < 9.0f || rc[2] < -0.8f){
NaotoMorita 143:53808e4e684c 33 pitchPID.setGain(4.55f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 34 pitchratePID.setGain(0.363f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 35 rollPID.setGain(1.34f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 36 rollratePID.setGain(0.219f, 0.0f,0.0f);
NaotoMorita 143:53808e4e684c 37 pitchPID.resetIntError();
NaotoMorita 143:53808e4e684c 38 rollPID.resetIntError();
NaotoMorita 143:53808e4e684c 39 }else{
NaotoMorita 143:53808e4e684c 40 pitchPID.setGain(4.55f, 9.7f, 0.0f);
NaotoMorita 143:53808e4e684c 41 pitchratePID.setGain(0.363f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 42 rollPID.setGain(1.34f, 0.007f, 0.0f);
NaotoMorita 143:53808e4e684c 43 rollratePID.setGain(0.219f, 0.0f,0.0f);
NaotoMorita 143:53808e4e684c 44 }
NaotoMorita 143:53808e4e684c 45 }else{
NaotoMorita 143:53808e4e684c 46 // 従来
NaotoMorita 143:53808e4e684c 47 hinf_flag = false;
NaotoMorita 143:53808e4e684c 48 if (vihat_norm < 9.0f || rc[2] < -0.8f){
NaotoMorita 143:53808e4e684c 49 pitchPID.setGain(10.0f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 50 pitchratePID.setGain(1.0f, 0.0f, 0.0f);//rad/s
NaotoMorita 143:53808e4e684c 51 rollPID.setGain(5.0f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 52 rollratePID.setGain(0.05f, 0.0f, 0.0f);//rad/s
NaotoMorita 143:53808e4e684c 53 pitchPID.resetIntError();
NaotoMorita 143:53808e4e684c 54 rollPID.resetIntError();
NaotoMorita 143:53808e4e684c 55 }else{
NaotoMorita 143:53808e4e684c 56 pitchPID.setGain(10.0f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 57 pitchratePID.setGain(1.0f, 0.0f, 0.0f);//rad/s
NaotoMorita 143:53808e4e684c 58 rollPID.setGain(5.0f, 0.0f, 0.0f);
NaotoMorita 143:53808e4e684c 59 rollratePID.setGain(0.05f, 0.0f, 0.0f);//rad/s
NaotoMorita 143:53808e4e684c 60 }
NaotoMorita 143:53808e4e684c 61 }
NaotoMorita 143:53808e4e684c 62
cocorlow 140:53dbdb207542 63 if (rc[4]>-0.3f && rc[6] < -0.3f)
cocorlow 140:53dbdb207542 64 {
cocorlow 140:53dbdb207542 65 //level_flight();
cocorlow 140:53dbdb207542 66 //point_guide();
NaotoMorita 143:53808e4e684c 67 //climb();
NaotoMorita 143:53808e4e684c 68
NaotoMorita 143:53808e4e684c 69 turning();
cocorlow 140:53dbdb207542 70 rollPID.setSetPoint(roll_obj);
cocorlow 140:53dbdb207542 71 pitchPID.setSetPoint(pitch_obj);
cocorlow 140:53dbdb207542 72 dT += dT_obj;
NaotoMorita 143:53808e4e684c 73 de = pitchPID.compute()-(rc[0]-rc[1])/2.0f;
NaotoMorita 143:53808e4e684c 74 da = rollPID.compute() -(rc[0]+rc[1])/2.0f;
cocorlow 140:53dbdb207542 75 }else{
NaotoMorita 143:53808e4e684c 76 yawratePID.setSetPoint(0.0f);
NaotoMorita 143:53808e4e684c 77 roll_obj = yawratePID.compute()*30.0f*M_PI/180.0f;
NaotoMorita 143:53808e4e684c 78 rollPID.setSetPoint(roll_obj);
NaotoMorita 143:53808e4e684c 79 climbratePID.setSetPoint(0.0f);
NaotoMorita 143:53808e4e684c 80 pitch_obj = -climbratePID.compute()*10.0f*M_PI/180.0f;
NaotoMorita 143:53808e4e684c 81 pitchPID.setSetPoint(pitch_obj);
NaotoMorita 143:53808e4e684c 82 de = pitchPID.compute() -(rc[0]-rc[1])/2.0f*2.0f;
NaotoMorita 143:53808e4e684c 83 da = rollPID.compute() -(rc[0]+rc[1])/2.0f*3.0f;
cocorlow 140:53dbdb207542 84 }
cocorlow 140:53dbdb207542 85
cocorlow 140:53dbdb207542 86 //舵角計算
cocorlow 140:53dbdb207542 87 if(rc[4]<-0.3f){
NaotoMorita 143:53808e4e684c 88 de = -(rc[0]-rc[1])/2.0f;
NaotoMorita 143:53808e4e684c 89 da = -(rc[0]+rc[1])/2.0f;
cocorlow 140:53dbdb207542 90 }
cocorlow 140:53dbdb207542 91
NaotoMorita 143:53808e4e684c 92 scaledServoOut[0]=-de-da;
NaotoMorita 143:53808e4e684c 93 scaledServoOut[1]=de-da;
cocorlow 140:53dbdb207542 94 scaledMotorOut[0]= dT;
cocorlow 140:53dbdb207542 95
NaotoMorita 143:53808e4e684c 96 float LP_servo = 1.0f;
NaotoMorita 143:53808e4e684c 97 float LP_motor = 0.2f;
cocorlow 140:53dbdb207542 98 for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
cocorlow 140:53dbdb207542 99 {
NaotoMorita 143:53808e4e684c 100 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
cocorlow 140:53dbdb207542 101 if(servoOut[i]<servoPwmMin)
cocorlow 140:53dbdb207542 102 {
cocorlow 140:53dbdb207542 103 servoOut[i] = servoPwmMin;
cocorlow 140:53dbdb207542 104 }
cocorlow 140:53dbdb207542 105 if(servoOut[i]>servoPwmMax)
cocorlow 140:53dbdb207542 106 {
cocorlow 140:53dbdb207542 107 servoOut[i] = servoPwmMax;
cocorlow 140:53dbdb207542 108 }
cocorlow 140:53dbdb207542 109 }
cocorlow 140:53dbdb207542 110
cocorlow 140:53dbdb207542 111 for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
NaotoMorita 143:53808e4e684c 112 motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
cocorlow 140:53dbdb207542 113 if(motorOut[i]<motorPwmMin) {
cocorlow 140:53dbdb207542 114 motorOut[i] = motorPwmMin;
cocorlow 140:53dbdb207542 115 };
cocorlow 140:53dbdb207542 116 if(motorOut[i]>motorPwmMax) {
cocorlow 140:53dbdb207542 117 motorOut[i] = motorPwmMax;
cocorlow 140:53dbdb207542 118 };
cocorlow 140:53dbdb207542 119 }
cocorlow 140:53dbdb207542 120 servoRight.pulsewidth_us(servoOut[0]);
cocorlow 140:53dbdb207542 121 servoLeft.pulsewidth_us(servoOut[1]);
cocorlow 140:53dbdb207542 122 servoThrust.pulsewidth_us(motorOut[0]);
cocorlow 140:53dbdb207542 123
NaotoMorita 143:53808e4e684c 124
cocorlow 140:53dbdb207542 125 sendData2PC();
cocorlow 140:53dbdb207542 126 writeSDcard();
cocorlow 140:53dbdb207542 127
cocorlow 140:53dbdb207542 128 if(loop_count >= 5)
cocorlow 140:53dbdb207542 129 {
cocorlow 140:53dbdb207542 130 sendTelemetry();
cocorlow 140:53dbdb207542 131 loop_count = 1;
cocorlow 140:53dbdb207542 132
cocorlow 140:53dbdb207542 133 }
cocorlow 140:53dbdb207542 134 else
cocorlow 140:53dbdb207542 135 {
cocorlow 140:53dbdb207542 136 loop_count +=1;
cocorlow 140:53dbdb207542 137 }
cocorlow 140:53dbdb207542 138 }