File content as of revision 127:d73a6233ee4b:

#include "global.hpp"

void run()
{
    wait(0.5);

    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    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);
    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.5f);
    eskf.setPhatAccBias(0.001f);
    eskf.setPhatGyroBias(0.001f);
    eskf.setPhatGravity(0.0000001f);
    
    eskf.setQVelocity(0.000025f);
    eskf.setQAngleError(0.00025f);
    eskf.setQAccBias(0.0000001f);
    eskf.setQGyroBias(0.000001f);
    
    Matrix Rgps(6,6);
    setDiag(Rgps,1.0f);
    //Rgps(4,4) = 0.1f;
    //Rgps(5,5) = 0.1f;
    
    Matrix Racc(3,3);
    setDiag(Racc,50.0f);

    Matrix Rheading(1,1);
    Rheading(1,1) = 0.01f;
    _t.start();
    float tgps = _t.read();
    float theading = _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();
        }
        
        headingUpdateFlag = false;
        if(tstart-theading>0.05f){
            Matrix euler = eskf.computeAngles();
            if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
                headingUpdateFlag = true;
                theading = _t.read();
            }
        }
        
        if(gpsUpdateFlag == true){
            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgps);
        }else if(headingUpdateFlag == true){
                float heading = atan2f(-mag.y,mag.x);
                eskf.updateHeading(heading,Rheading);
        }else{
            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
        }
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}