File content as of revision 75:7fa924da9d32:

#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]);
    
    // 自身の位置に応じてエレベータ舵角を決定する
    float derc =  rc[1];
    float darc =  rc[0]-rc[1];
    deobj = 0.0f;
    switch(pos_tail)
    {
    case 0: //left
        deobj = derc-darc;
        break;
    case 1: //center
        deobj = derc;
        break;
    case 2: //right
        deobj = derc+darc;
        break;
    default:   // error situation
        deobj = 0.0f;
        break;
    }
    
    
    float objgain =  15.0f*M_PI/180.0f;
    float pitchobj = objgain * (deobj+updateValues.de_command);
    pc.printf("%f %f \r\n",rpy.x/M_PI*180.0f,rpy.y/M_PI*180.0f);
    //ゲインの係数をrc8で変更
    float gaincoef = 1.0f;
    if(gaincoef<0.0f){gaincoef = 0.0f;}
    if(gaincoef>1.0f){gaincoef = 1.0f;}
    pitchPID.setGain(6.36, 10.6*gaincoef*0.0f,0.0);
    pitchratePID.setGain(0.9540,0.0,0.0);
    pitchPID.setProcessValue(rpy.x);
    pitchratePID.setProcessValue(gyro.x);
    //目標値のセット
    pitchPID.setSetPoint(pitchobj); //目標の設定
    
    de = pitchPID.compute()+pitchratePID.compute();
    if(de<-1.0f){de = -1.0f;}
    if(de>1.0f){de = 1.0f;}
    
    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]);
    send2center();
}