File content as of revision 84:ff48e01ea76b:

#include "global.hpp"

void run()
{
    //pc.serial.printf("\r\nrun Mode\r\n");
    wait(0.5);
    Timer _t;
    _t.start();
    magCalibrator.setExtremes(magbiasMin,magbiasMax);

    float sum2accnorm = 0;
    float sumaccnorm = 0;
    int n_init = 1000;
    if(hilFlag == false){
        ekf.defineQhat(rpy_align);
        for(int i = 0; i < n_init; i++){
            float tstart = _t.read();
            if(hilFlag == true){
                getHILval();
            }else{
                getIMUval();
            }
            ekf.updateStaticAccMeasures(acc,accref);
            ekf.updateGyroBiasConstraints(gyro);
            ekf.fuseErr2Nominal();
            ekf.resetBias();
            //ekf.updateMagMeasures(mag);
            ekf.computeAngles(rpy, rpy_align);
            sumaccnorm += acc.Norm();
            sum2accnorm += acc.Norm()*acc.Norm();
            float tend = _t.read();
            att_dt = (tend-tstart);
        }
        accref.z =  sumaccnorm / float(n_init);
    }
    pc.Subscribe(0000, &(vp));
    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    

    
    while(1)
    {
        float tstart = _t.read();
        //姿勢角を更新
        if(hilFlag == true){
            getHILval();
        }else{
            getIMUval();
        }

        ekf.updateNominal(gyro,acc,accref,att_dt);
        ekf.updateErrState(gyro,acc, att_dt);
        if(obsCount == 50){
            ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
            obsCount = 0;
        }else{
            if(ekf.determinDynStatus(acc,accref)){
                //ekf.updateAccMeasures(acc,accref);
            }else{   
                //ekf.updateStaticAccMeasures(acc,accref);
            }
            obsCount += 1;
        }
        //ekf.updateSinkRate(vi.z,acc,accref);
        ekf.updateGyroBiasConstraints(gyro);
        
        ekf.fuseErr2Nominal();
        ekf.resetBias();
        ekf.computeAngles(rpy, rpy_align);
        ekf.computeVb(vb);
        PIDtick.loop(); 
        
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}

    /*
    if(serialParamSource){
        while(1){
            pc.serial.attach(NULL, Serial::RxIrq);
            pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]);
            pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq);
            switch(vp.commandIndex){
            case 1:
                NVIC_SystemReset();
                break;
            case 10:
                ekf.setQqerr(float(vp.commandVal));
                checkParamSerial[0] = 1;
                break;
            case 11:
                ekf.setQgbias(float(vp.commandVal));
                checkParamSerial[1] = 1;
                break;
            case 12:
                Qabdyn=float(vp.commandVal);
                checkParamSerial[2] = 1;
                break;
            case 13:
                Rscdyn=float(vp.commandVal);
                checkParamSerial[3] = 1;
                break;
            case 14:
                
                checkParamSerial[4] = 1;
                break;
            default :
                break;
            }
            int cpsSum = 0;
            for(int i = 0;i<5;i++){
                cpsSum +=  checkParamSerial[i];
            }
            if(cpsSum == 5){
                break;
            }
            wait(0.01);
        }   
    }
    */