File content as of revision 69:0caaad87cf1d:

#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]);
     
    if(loop_count >= 3)
    {
        writeSdcard();
        loop_count = 1;
        obs_count = 0;
    }
    else
    {
        loop_count +=1;
    }
}