File content as of revision 95:43717535c354:

#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); 
    }
    for(int i = 0;i<6;i++){
        agoffset[i] /= 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);
    
    Timer _t;
    _t.start();
    
    //EKFの共分散設定
    eskf.setGravity(0.0f,0.0f,9.8f);
    eskf.setPhatPosition(0.1f);
    eskf.setPhatVelocity(0.1f);
    eskf.setPhatAngleError(0.1f);
    eskf.setPhatAccBias(0.5f);
    eskf.setPhatGyroBias(0.5f);
    eskf.setPhatGravity(0.1f);
    
    eskf.setQVelocity(0.0025f);
    eskf.setQAngleError(0.00025f);
    eskf.setQAccBias(0.000001f);
    eskf.setQGyroBias(0.000001f);
    
    Matrix Rgpspos(3,3);
    setDiag(Rgpspos,0.9f);
    Matrix Rgpsvel(3,3);
    setDiag(Rgpsvel,0.1f);
    
    Matrix RaccConst(3,3);
    setDiag(RaccConst,400.0f);
    //Matrix RgyroConst(2,2);
    //setDiag(RgyroConst,100);
    
    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(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.fuseErr2Nominal();
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}