File content as of revision 144:b3a713b4f1c4:

#include "global.hpp"

void run()
{
    preflightCalibration();
    
    setEskfCov();
    
    MatrixXf Rgps(5,5);
    setDiag(Rgps,1.5f);
    Rgps(2,2) = 0.1f;
    Rgps(3,3) = 0.1f;
    Rgps(4,4) = 0.1f;
    
    MatrixXf Rwhole = MatrixXf::Zero(9,9);
    Rwhole(0,0) = 1.5f;
    Rwhole(1,1) = 1.5f;
    Rwhole(2,2) = 0.1f;
    Rwhole(3,3) = 0.1f;
    Rwhole(4,4) = 0.1f;
    Rwhole(5,5) = 5000.0f;
    Rwhole(6,6) = 5000.0f;
    Rwhole(7,7) = 5000.0f;
    Rwhole(8,8) = 0.007f;
    
    MatrixXf Rimu = MatrixXf::Zero(5,5);
    Rimu(0,0) = 0.1f;
    Rimu(1,1) = 5000.0f;
    Rimu(2,2) = 5000.0f;
    Rimu(3,3) = 5000.0f;
    Rimu(4,4) = 0.007f;
    
    
    _t.start();

    preflightCheck();
    wait(1.0f);
    //usaPack通信開始 制御ループのアタッチ
    pc.Subscribe(0000, &(vp));
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    float tgps = _t.read();
    float theading = _t.read();
    float tstart = _t.read();
    while(1)
    {
        float tstart = _t.read();
        //センサの値を取得
        if(hilFlag == true){
            getHilIMUval();
        }else{
            getIMUval();
        }
        //ekfの更新
        eskf.updateNominal(acc, gyro, att_dt);
        eskf.updateErrState(acc, gyro, att_dt);

        if(hilFlag == true){
            if(tstart-tgps>0.2f){
                getHilGPSval();
                tgps = _t.read();
                gpsUpdateFlag = true;
            }else{
                gpsUpdateFlag = false;
            }
        }else{
            if(userButton.read()==1)
            {
                gpsLlh0Fixed = false;
            };  
            getGPSval();
        }
        
        float heading = std::atan2(-mag(1),mag(0));
        Vector3f dynacc = eskf.calcDynAcc(acc);
        dynaccnorm2 = dynacc.squaredNorm();
        if(dynaccnorm2 > 0.16f){
            Rimu(1,1) = 1.1f;
            Rimu(2,2) = 1.1f;
            Rimu(3,3) = 1.1f;
        }else{
            Rimu(1,1) = 100.1f;
            Rimu(2,2) = 100.1f;
            Rimu(3,3) = 100.1f;
        }
        eskf.updateIMU(palt,acc,heading, Rimu); 
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}