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:
- 2021-06-03
- Revision:
- 61:c05353850017
- Parent:
- 57:923e3df16159
- Child:
- 62:ef10fd919f7b
File content as of revision 61:c05353850017:
#include "global.hpp" // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut() { if(sbus.failSafe == false) { // sbusデータの読み込み 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 } } // 自身の位置に応じてエレベータ舵角を決定する float deobj = 0.0f; switch(pos_tail) { case 0: //left deobj = -rc[1]; break; case 1: //center deobj = -rc[1]; break; case 2: //right deobj = -rc[1]; break; default: // error situation deobj = 0.0f; break; } //指令角の最大最小値をrc7で変更 float objgain = ((rc[6]+1.0f)/2*10.0f+1.0f)*M_PI/180.0f; float pitchobj = objgain * deobj; //ゲインの係数をrc8で変更 float gaincoef = ((rc[7]+1.0f)/2*0.9)+0.1; pitchPID.setGain(6.36*gaincoef, 10.6*gaincoef,0.0); pitchratePID.setGain(0.9540*gaincoef,0.0,0.0); pitchPID.setProcessValue(rpy.y); pitchratePID.setProcessValue(gyro.y); //目標値のセット pitchPID.setSetPoint(pitchobj); //目標の設定 float de = pitchPID.compute()+pitchratePID.compute(); scaledServoOut[0]=de; float LP_servo = 0.2; 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; } } servo.pulsewidth_us(servoOut[0]); //観測アップデート dynacc = ekf.calcDynAcc(LPacc, accref); th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg); accnormerr = abs(LPacc.Norm()-accref.Norm()); //静止時100個の平均 0.01877522 0.00514146 0.00477393 //int ang_th = th_mg < 0.01877522; //int dyn_th = dynaccnorm < 0.00514146; //int norm_th = accnormerr< 0.00477393; int ang_th = th_mg < sigma_thmg/200.0; int dyn_th = dynacc.Norm() < sigma_accnorm/50.0; int norm_th = accnormerr< sigma_accnorm/50.0; if(dyn_th+ang_th+norm_th>0) { //if(dyn_th+ang_th+norm_th>-1){ obs_count += 1; ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag); ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc); } if(loop_count >= 10) { writeSdcard(); loop_count = 1; obs_count = 0; } else { loop_count +=1; } }