File content as of revision 87:89bbbcdb667b:

#include "global.hpp"

void run()
{
    wait(0.5);
    Timer _t;
    _t.start();

    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    float sumaccnorm = 0;
    int n_init = 1000;
    if(hilFlag == false){
        ekf.defineQhat(rpy_align);
        for(int i = 0; i < n_init; i++){
            sumaccnorm += acc.Norm();
        }
        accref.z =  sumaccnorm / float(n_init);
    }
    
    //センサ正常性チェック

    //usaPack通信開始
    pc.Subscribe(0000, &(vp));
    
    //制御ループのアタッチ
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    while(1)
    {
        float tstart = _t.read();
        //センサの値を取得
        if(hilFlag == true){
            getHILval();
        }else{
            //getIMUval();
        }

        //ekfの更新
        ekf.updateNominal(gyro,acc,accref,att_dt);
        ekf.updateErrState(gyro,acc, att_dt);
        if(obsCount == 50){
            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
            //ekf.updateSinkRate(vi.z,acc,accref);
            obsCount = 0;
        }else{
            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
            //ekf.updateAccMeasures(acc,accref);
            
            if(ekf.determinDynStatus(acc,accref)){
                ekf.updateAccMeasures(acc,accref);
            }else{   
                ekf.updateStaticAccMeasures(acc,accref);
            }
            
            obsCount += 1;
        }
        //ekf.updateSinkRate(vi.z,acc,accref);
        //ekf.updateGyroBiasConstraints(gyro);
        
        ekf.fuseErr2Nominal();
        ekf.resetBias();
        ekf.computeAngles(rpy, rpy_align);
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}