File content as of revision 68:b9f6938fab9d:

#include "global.hpp"

// 割り込まれた時点での出力(computeの結果）を返す関数
void calcServoOut()
{
    
    // 自身の位置に応じてエレベータ舵角を決定する
    float deobj = 0.0f;
    float drobj = 0.0f;
    
    //指令角の最大最小値をrc7で変更
    float pitchobj = 10.0f*M_PI/180.0f * 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); //目標の設定
    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++)
    {
        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;
        }
    }
    
    elevServo.pulsewidth_us(servoOut[0]);
    rudServo.pulsewidth_us(servoOut[1]);
 

    if(loop_count >= 10)
    {
        writeSdcard();
        loop_count = 1;

    }
    else
    {
        loop_count +=1;
    }
}