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:
- 66:e5afad70fdd8
- Parent:
- 62:ef10fd919f7b
- Child:
- 68:b9f6938fab9d
diff -r ea184054e659 -r e5afad70fdd8 servo.cpp --- a/servo.cpp Tue Jun 22 02:08:31 2021 +0000 +++ b/servo.cpp Tue Jun 22 02:19:14 2021 +0000 @@ -3,36 +3,13 @@ // 割り込まれた時点での出力(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 - } - } - //pc.printf("rc:%f\r\n",rc[1]); // 自身の位置に応じてエレベータ舵角を決定する 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; - } + float drobj = 0.0f; + //指令角の最大最小値をrc7で変更 - float objgain = ((rc[6]+1.0f)/2*10.0f+1.0f)*M_PI/180.0f; - float pitchobj = objgain * deobj; + float pitchobj = 10.0f*M_PI/180.0f * deobj; //ゲインの係数をrc8で変更 float gaincoef = 1.0; @@ -43,9 +20,9 @@ //目標値のセット pitchPID.setSetPoint(pitchobj); //目標の設定 float de = pitchPID.compute()+pitchratePID.compute(); - + float dr = 0.0f; scaledServoOut[0]=de; - + scaledServoOut[1]=dr; float LP_servo = 0.2; for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++) { @@ -60,34 +37,13 @@ } } - 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); - } + elevServo.pulsewidth_us(servoOut[0]); + rudServo.pulsewidth_us(servoOut[1]); if(loop_count >= 10) { writeSdcard(); loop_count = 1; - obs_count = 0; } else {