File content as of revision 144:b3a713b4f1c4:

#include "global.hpp"

// 割り込まれた時点での出力(computeの結果）を返す関数
void calcServoOut()
{
    // sbusデータの読み込み
    for (int i =0 ; i < 16;i ++){
        rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input
    }

    //姿勢角の所得
    euler = eskf.computeAngles();
    euler(1) -= 3.0f*M_PI_F/180.0f;
    Vector3f gyroBias = eskf.getGyroBias();
    //PIDへの状態量のセット
    pitchPID.setProcessValue(euler(1));
    pitchratePID.setProcessValue(gyro(1)-gyroBias(1));
    rollPID.setProcessValue(euler(0));
    rollratePID.setProcessValue(gyro(0)-gyroBias(0));
    yawratePID.setProcessValue(gyro(2)-gyroBias(2));
    
    vxPID.setGain(0.005f,0.0f,0.0f);
    vyPID.setGain(0.005f,0.0f,0.0f);

    
    pitchPID.setGain(20.0f,0.0f,0.0f);
    pitchratePID.setGain(0.18f,0.0f,0.0f);
    rollPID.setGain(20.0f,0.0f,0.0f);
    rollratePID.setGain(0.18f,0.0f,0.0f);
    
    Vector3f Vb = eskf.calcVb();
    Vector3f dynAcc = eskf.calcDynAcc(acc);
    vxPID.setProcessValue(dynAcc(0));
    vxPID.setSetPoint(0.0f);
    vyPID.setProcessValue(dynAcc(1));
    vyPID.setSetPoint(0.0f);
    vzPID.setProcessValue(Vb(2));
    vzPID.setSetPoint(0.0f);
    
    //dT = -vzPID.compute()+rc[2];
    dT = +rc[2];
    
    float pitchobj = rc[1]* 30.0f*M_PI_F/180.0f;
    float rollobj = rc[0] * 30.0f*M_PI_F/180.0f;
    yawratePID.setSetPoint(0.0f);
    rollPID.setSetPoint(rollobj);
    pitchPID.setSetPoint(pitchobj);
    
    float pitchrateobj = pitchPID.compute()* 30.0f*M_PI_F/180.0f;
    float rollrateobj =  rollPID.compute()* 30.0f*M_PI_F/180.0f;
    pitchratePID.setSetPoint(pitchrateobj);
    rollratePID.setSetPoint(rollrateobj);
    
    de = pitchratePID.compute();
    da = rollratePID.compute();
    dr = -0.1f*rc[3]+yawratePID.compute();
    
    scaledMotorOut[0]= dT+de-dr;
    scaledMotorOut[1]= dT+0.866f*de-0.866f*da+dr;
    scaledMotorOut[2]= dT-0.866f*de-0.866f*da-dr;
    scaledMotorOut[3]= dT-de+dr;
    scaledMotorOut[4]= dT-0.866f*de+0.866f*da-dr;
    scaledMotorOut[5]= dT+0.866f*de+0.866f*da+dr;
    
    float minArmThrottle = 0.25f;
    for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){
      if(scaledMotorOut[i]<-1.0f+minArmThrottle) {
          scaledMotorOut[i] = -1.0f+minArmThrottle;
      }
    }
    
    //pc.serial.printf("%f %f %f %f\r\n",dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
    
    float LP_motor = 1.0f;
    for(int i = 0;i<6 ;i++){
        motorOut[i] = (mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax));
        if(motorOut[i]<motorPwmMin) {
            motorOut[i] = motorPwmMin;
        };
        if(motorOut[i]>motorPwmMax) {
            motorOut[i] = motorPwmMax;
        };
    }
    
    if(rc[7]>0.3f){
        motor1.pulsewidth_us(motorOut[0]);
        motor2.pulsewidth_us(motorOut[1]);
        motor3.pulsewidth_us(motorOut[2]);
        motor4.pulsewidth_us(motorOut[3]);
        motor5.pulsewidth_us(motorOut[4]);
        motor6.pulsewidth_us(motorOut[5]);
    }else{
        motor1.pulsewidth_us(motorPwmMin);
        motor2.pulsewidth_us(motorPwmMin);
        motor3.pulsewidth_us(motorPwmMin);
        motor4.pulsewidth_us(motorPwmMin);
        motor5.pulsewidth_us(motorPwmMin);
        motor6.pulsewidth_us(motorPwmMin);
    }
    sendData2PC();
    writeSDcard();
    
    if(loop_count >= 5)
    {
        sendTelemetry();
        loop_count = 1;

    }
    else
    {
        loop_count +=1;
    }
}