File content as of revision 115:ceb9ef04bb26:

#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]);
    
    //姿勢角の取得
    euler = eskf.computeAngles();
    Matrix gyroBias = eskf.getGyroBias();
    rpy.x = euler(1,1);
    rpy.y = euler(2,1);
    rpy.z = euler(3,1);
    
    if (updateValues.calibrationFlag == 1111)
    {
        alignment();
    }
    else
    {
        rpy.x -= roll_offset;
        rpy.y -= pitch_offset;
    }
    
    // 自身の位置に応じてエレベータ舵角を決定する
    float derc =  -rc[1];
    //pc.printf("derc: %f\r\n", derc);
    float darc =  -rc[0] + rc[1];
    //pc.printf("darc: %f\r\n", darc);
    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;
    }
    
    drobj = rc[3];
    
    
    float objgain =  15.0f*M_PI/180.0f;
    float pitchobj = objgain * (deobj + updateValues.de_command);
    if (pitchobj > objgain)
    {
        pitchobj = objgain;
    }
    if (pitchobj < -objgain)
    {
        pitchobj = -objgain;
    }
    pc.printf("pitchobj: %f\r\n", pitchobj);
    if(rc[7] < -0.3f){
        Matrix vihat = eskf.getVihat();
        float vihat_norm = vihat(1, 1) * vihat(1, 1) + vihat(2, 1) * vihat(2, 1) + vihat(3, 1) * vihat(3, 1);
        pc.printf("rc[2]: %f\r\n", rc[2]);
        if (vihat_norm > 9.0f || rc[2] > -0.8f)
        {
            pitchPID.setGain(6.36f, 10.6f*0.5f,0.0);
            pitchratePID.setGain(0.9540f,0.0f,0.0f);
        }
        else
        {
            pitchPID.setGain(6.36f, 0.0f,0.0);
            pitchPID.resetIntError();
            pitchratePID.setGain(0.9540f,0.0f,0.0f);
        }
    }else{
        pitchPID.setGain(6.36f, 0.0f,0.0);
        pitchPID.resetIntError();
        pitchratePID.setGain(0.9540f,0.0f,0.0f);
    }
    yawratePID.setGain(2.0f,0.0f,0.0f);
    pitchPID.setProcessValue(rpy.y);
    pitchratePID.setProcessValue((gyro.y-gyroBias(2,1)));
    yawratePID.setProcessValue(gyro.z-gyroBias(3,1));
    //目標値のセット
    pitchPID.setSetPoint(pitchobj); //目標の設定
    
    de = pitchPID.compute()+pitchratePID.compute();
    if(de<-1.0f){de = -1.0f;}
    if(de>1.0f){de = 1.0f;}
    pc.printf("%f %f %f %f %f\r\n",pitchobj,deobj,de,drobj,dr);
    
    dr = yawratePID.compute()+drobj;
    if(dr<-1.0f){dr = -1.0f;}
    if(dr>1.0f){dr = 1.0f;}

    scaledServoOut[0]=-de;
    scaledServoOut[1]=dr;
    scaledMotorOut[0]=rc[2]+drobj*0.2f+updateValues.dT_command;
    scaledMotorOut[1]=rc[2]-drobj*0.2f+updateValues.dT_command;
    
    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;
        }
    }
    
    float LP_motor = 0.2;
    for(int i = 0; i < sizeof(motorOut)/sizeof(motorOut[0]); i++)
    {
        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
        if(motorOut[i]<motorPwmMin)
        {
            motorOut[i] = motorPwmMin;
        }
        if(motorOut[i]>motorPwmMax)
        {
            motorOut[i] = motorPwmMax;
        }
    }
    
    servo.pulsewidth_us(servoOut[0]);
    rudServo.pulsewidth_us(servoOut[1]);
    motor1.pulsewidth_us(motorOut[0]);
    motor2.pulsewidth_us(motorOut[1]);
    send2center();
}