File content as of revision 56:888379912f81:

#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
        }
    }
    pitchPID.setGain(6.36, 10.6,0.0);
    pitchratePID.setGain(0.9540,0.0,0.0);
    pitchPID.setProcessValue(rpy.y);
    pitchratePID.setProcessValue(gyro.y);
    float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
    
    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]);
     
    //観測アップデート
    dynacc = ekf.calcDynAcc(LPacc, accref);
    th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg);
    
    accnormerr = abs(LPacc.Norm()-accref.Norm());
    //静止時100個の平均　0.01877522 0.00514146  0.00477393

    //int ang_th  = th_mg < 0.01877522;
    //int dyn_th = dynaccnorm < 0.00514146;
    //int norm_th =  accnormerr< 0.00477393;
    int ang_th  = th_mg < 0.01877522/5.0;
    int dyn_th = dynacc.Norm() < 0.00514146/5.0;
    int norm_th =  accnormerr< 0.00477393/5.0;
    if(dyn_th+ang_th+norm_th>0)
    {
    //if(dyn_th+ang_th+norm_th>-1){
            obs_count += 1;
            ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
            ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
    } 
     
    if(loop_count >= 10)
    {
        writeSdcard();
        loop_count = 1;
        obs_count = 0;
    }
    else
    {
        loop_count +=1;
    }
}