File content as of revision 68:11ffadc7adb1:

#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
        }
    }
    //pc.printf("rc:%f\r\n",rc[1]);
    
    // 自身の位置に応じてエレベータ舵角を決定する
    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 = 1.0;
    pitchPID.setGain(6.36*gaincoef, 10.6*gaincoef*0.0f,0.0);
    pitchratePID.setGain(0.9540*gaincoef,0.0,0.0);
    pitchPID.setProcessValue(rpy.y);
    pitchratePID.setProcessValue(gyro.y);
    //目標値のセット
    pitchPID.setSetPoint(pitchobj); //目標の設定
    de = pitchPID.compute()+pitchratePID.compute();
    
    if (pos_tail == 0)
    {
        
        scaledServoOut[0]=de;
    }
    else
    {
        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) 
    if(loop_count >= 3)
    {
        writeSdcard();
        loop_count = 1;
        obs_count = 0;
    }
    else
    {
        loop_count +=1;
    }
}