File content as of revision 120:9f4725deb5a6:

#include "global.hpp"

void run()
{
    
    preflightCalibration();
    
    //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.001f);
    eskf.setQAngleError(0.0000025f);
    eskf.setQAccBias(0.000001f);
    eskf.setQGyroBias(0.000001f);
    
    Matrix Rgps(5,5);
    setDiag(Rgps,0.05f);
    Rgps(4,4) = 0.1f;
    Rgps(5,5) = 0.1f;
    
    float dynAccCriteria = 0.4f;
    Matrix Racc(3,3);
    setDiag(Racc,100.0f);
    Matrix RaccDyn(3,3);
    setDiag(RaccDyn,5000.0f);

    Matrix Rheading(1,1);
    Rheading(1,1) = 0.01f;
    
    Matrix Ropt(2,2);
    setDiag(Ropt,10.0f);
    
    wait(0.5);
    
    _t.start();
    tail.Subscribe(tail_address[pos_tail], &(updateValues));
    pc.Subscribe(0000,&(rp));
    
    if(hilFlag == false){preflightCheck();};

    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    while(1)
    {
        tstart = _t.read();
        //センサの値を取得
        if(hilFlag==false){
            getIMUval();
        }else{
            getHilIMUval();
        }
        
        calcOpticalVel();
        
        //ekfの更新
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        Matrix pihat = eskf.getPihat();
        //pc.printf("pihat: %f %f %f\r\n", pihat(1, 1), pihat(2, 1), pihat(3, 1));
        
        //キャリブレーション中
        if(updateValues.calibrationFlag == 1111){
            mainloopCalibration();
        }else{
            gpsUpdateFlag = false;
            //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );
            if(hilFlag==false){
                if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
                    getGPSval();
                    gpsUpdateFlag = true;
                    gpsitow = updateValues.gps_itow;
                    tgps = _t.read();
                }
            }else{
                if(tstart-tgps>0.2f){
                    getHilGPSval();
                    tgps = _t.read();
                    gpsUpdateFlag = true;
                }
            }
            
            sensorUpdateFlag = false;
            if(tstart-tsensors>0.05f){
                sensorUpdateFlag = true;
                tsensors = _t.read();
            }
            
            if(gpsUpdateFlag==true){
                Rgps(1, 1) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
                Rgps(2, 2) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
                eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
                //pc.printf("gps\r\n");
            }else if(sensorUpdateFlag == true){
                if(sensorUpdateID==1){
                    Matrix euler = eskf.computeAngles();
                    if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
                        //float heading = atan2f(-mag.y,mag.x);
                        eskf.updateHeading(updateValues.heading,Rheading);
                        //pc.printf("heading\r\n");
                    }
                    sensorUpdateID += 1;
                }else{
                    //float vnorm_opt2 = updateValues.vx_opt*updateValues.vx_opt+updateValues.vy_opt*updateValues.vy_opt;
                    //if(updateValues.dist_ir<1.0f && vnorm_opt2 < 400.0f){
                        //if(vnorm_opt2 < 0.1f){
                            //Matrix vbOpt(2,1);
                            //vbOpt(1,1) = updateValues.vx_opt;
                            //vbOpt(2,1) = updateValues.vy_opt;
                            //eskf.updateVb(vbOpt,Ropt*0.1f);
                        //}else{
                            //Matrix vbOpt(2,1);
                            //vbOpt(1,1) = updateValues.vx_opt;
                            //vbOpt(2,1) = updateValues.vy_opt;
                            //eskf.updateVb(vbOpt,Ropt);
                        //}
                        //pc.printf("opt\r\n");
                    //}
                    sensorUpdateID=1;
                }
            }else{
                Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
                dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
                if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
                    eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
                    //pc.printf("dyn acc\r\n");
                }else{
                    eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
                    //pc.printf("static acc\r\n");
                    }
                
            }

            
        }
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}