File content as of revision 139:b378528c05f2:

//#include "global.hpp"
//
//// 割り込まれた時点での出力(computeの結果）を返す関数
//void calcServoOut()
//{
//    // 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
//    }
//
//    
//    //姿勢角の所得
//    euler = eskf.computeAngles();
//    rpy.x = euler(1,1);
//    rpy.y = euler(2,1);
//    rpy.z = euler(3,1);
//
//    //PIDへの状態量のセット
//    pitchPID.setProcessValue(rpy.y);
//    pitchratePID.setProcessValue(gyro.y);
//    rollPID.setProcessValue(rpy.x);
//    rollratePID.setProcessValue(gyro.x);
//    
//    dT = rc[2];
//    
//    if (rc[4]>-0.3f && rc[6] < -0.3f)
//    {
//        //level_flight();
//        //point_guide();
//        climb();
//        rollPID.setSetPoint(roll_obj);
//        pitchPID.setSetPoint(pitch_obj);
//        dT += dT_obj;
//    }else{
//        rollPID.setSetPoint(0.0f);
//        pitchPID.setSetPoint(0.0f);
//    }
//    
//    //舵角計算
//    if(rc[4]<-0.3f){
//        de = (rc[0]-rc[1])/2.0f;
//        da = (rc[0]+rc[1])/2.0f;
//    }else{
//        de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
//        da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
//    }
//    
//    scaledServoOut[0]=de+da;
//    scaledServoOut[1]=-de+da;
//    scaledMotorOut[0]= dT;
//    
//    float LP_servo = 0.2;
//    float LP_motor = 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;
//        }
//    }
//    
//    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;
//        };
//    }
//    servoRight.pulsewidth_us(servoOut[0]);
//    servoLeft.pulsewidth_us(servoOut[1]); 
//    servoThrust.pulsewidth_us(motorOut[0]);
//
//    sendData2PC();
//    writeSDcard();
//    
//    if(loop_count >= 5)
//    {
//        sendTelemetry();
//        loop_count = 1;
//
//    }
//    else
//    {
//        loop_count +=1;
//    }
//}