File content as of revision 105:aaaed895ffaf:

#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));
    
    preflightCheck();
    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    while(1)
    {
        tstart = _t.read();
        //センサの値を取得
        getIMUval();
        calcOpticalVel();
        
        //ekfの更新
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        
        //キャリブレーション中
        if(updateValues.calibrationFlag == 1111){
            mainloopCalibration();
        }else{
            gpsUpdateFlag = false;
            //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );
            if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
                gpsUpdateFlag = true;
                gpsitow = updateValues.gps_itow;
                tgps = _t.read();
            }
            
            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){
                Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
                Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
                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(headingUpdateFlag == true){
                    float heading = atan2f(-mag.y,mag.x);
                    eskf.updateHeading(heading,Rheading);
            }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);
                }else{
                    eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
                }
            }
            if(updateValues.dist_ir<1.0f){
                Matrix vbOpt(2,1);
                vbOpt(1,1) = opt_vx;
                vbOpt(2,1) = opt_vy;
                eskf.updateVb(vbOpt,Ropt);
            }
            
        }
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}