File content as of revision 104:20b8caa29185:

#include "global.hpp"

void run()
{
    wait(0.5);

    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    if(hilFlag == false){
        eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
    }
    
    int n_init = 1000;
    for(int i = 0;i<n_init;i++){
        lsm.readAccel();
        lsm.readMag();
        lsm.readGyro();
        agoffset[0] += lsm.ax * 9.8f;
        agoffset[1] += lsm.ay * 9.8f;
        agoffset[2] += lsm.az * 9.8f-9.8f;
        agoffset[3] +=(lsm.gx * M_PI / 180.0f);
        agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
        agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
        magref.x += lsm.mx;
        magref.y += lsm.my;
        magref.z += lsm.mz;
    }
    for(int i = 0;i<6;i++){
        agoffset[i] /= float(n_init);
    }
    magref.x /= float(n_init);
    magref.y /= float(n_init);
    magref.z /= float(n_init);
    if(hilFlag == false){
        eskf.setMagField(magref.x,magref.y,-magref.z);
    }else{
        eskf.setMagField(magref.x,magref.y,magref.z);
    }
    gps.Update();
    gps.CalculateUnit();
    palt0 /= float(n_init);
    twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
    //センサ正常性チェック
    //usaPack通信開始
    pc.Subscribe(0000, &(vp));
    
    //制御ループのアタッチ
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    //ESKFの共分散設定
    eskf.setGravity(0.0f,0.0f,9.8f);
    eskf.setPhatPosition(0.1f);
    eskf.setPhatVelocity(0.1f);
    eskf.setPhatAngleError(0.1f);
    eskf.setPhatAccBias(0.001f);
    eskf.setPhatGyroBias(0.001f);
    eskf.setPhatGravity(0.0000001f);
    eskf.setPhatMagField(0.00001f);
    
    eskf.setQVelocity(0.0025f);
    eskf.setQAngleError(0.00025f);
    eskf.setQAccBias(0.0000001f);
    eskf.setQGyroBias(0.000001f);
    eskf.setQMagField(0.001f);
    
    Matrix Rgps(5,5);
    setDiag(Rgps,2.0f);
    
    Matrix RImuConst(5,5);
    setDiag(RImuConst,50.0f);
    RImuConst(4,4) = 1.0f;
    RImuConst(5,5) = 1.0f;
    _t.start();
    float tgps = _t.read();
    while(1)
    {
        float tstart = _t.read();
        //センサの値を取得
        if(hilFlag == true){
            getHilIMUval();
        }else{
            getIMUval();
        }
        //ekfの更新
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);

        
        if(hilFlag == true){
            if(tstart-tgps>0.2f){
                getHilGPSval();
                tgps = _t.read();
                gpsUpdateFlag = true;
            }else{
                gpsUpdateFlag = false;
            }
        }else{
            getGPSval();
        }
        if(gpsUpdateFlag == true){
            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
            
        }else{
            eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
        }
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}