File content as of revision 92:00460f6df439:

#include "global.hpp"

void run()
{
    wait(0.5);

    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    if(hilFlag == false){
        eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
    }
    
    //センサ正常性チェック
    //usaPack通信開始
    pc.Subscribe(0000, &(vp));
    
    //制御ループのアタッチ
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    Timer _t;
    _t.start();
    
    float tgps = _t.read();
    Matrix Rgpspos(3,3);
    Rgpspos(1,1) = 5000.0f;
    Rgpspos(2,2) = 5000.0f;
    Rgpspos(3,3) = 5000.0f;
    Matrix Rgpsvel(3,3);
    Rgpsvel(1,1) = 5000.0f;
    Rgpsvel(2,2) = 5000.0f;
    Rgpsvel(3,3) = 5000.0f;
    Matrix RaccConst(3,3);
    RaccConst(1,1) = 980000.0f;
    RaccConst(2,2) = 980000.0f;
    RaccConst(3,3) = 980000.0f;
    Matrix RgyroConst(2,2);
    RgyroConst(1,1) = 100000.0f;
    RgyroConst(2,2) = 100000.0f;
    while(1)
    {
        float tstart = _t.read();
        //センサの値を取得
        if(hilFlag == true){
            getHilIMUval();
            getIMUval();
        }else{
            //getIMUval();
        }
        //ekfの更新
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        if(tstart-tgps>0.2f){
            if(hilFlag == true){
                getHilGPSval();
            }else{
                //getGPSval();
            }
            eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
            eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
            tgps = _t.read();
        }else{
            //eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst);
            //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
            eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
            eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
        }
        eskf.fuseErr2Nominal();
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}