File content as of revision 75:a505b9896da1:

#include "global.hpp"

void run()
{
    pc.serial.printf("\r\nrun Mode\r\n");
    wait(0.5);
    Timer _t;
    _t.start();
    magCalibrator.setExtremes(magbiasMin,magbiasMax);
    ekf.defineQhat(rpy_align);
    float sum2accnorm = 0;
    float sumaccnorm = 0;
    for(int i = 0; i < 1000; i++){
        float tstart = _t.read();
        if(hilFlag == true){
            getHILval();
        }else{
            getIMUval();
        }
        //ekf.updateQhat(gyro, att_dt);
        //ekf.updateErrState(gyro, att_dt);
        ekf.updateStaticAccMeasures(acc,accref);
        //ekf.fuseErr2Qhat();
        //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 / 1000.0f;
    float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
    pc.serial.printf("%f \r\n",sigma_accnorm);
    pc.Subscribe(0000, &(vp));
    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    

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

        ekf.updateQhat(gyro, att_dt);
        ekf.updateErrState(gyro, att_dt);
        //ekf.updateAllMeasures(acc,gyro,mag,accref,tstart,att_dt);
        if(ekf.determinDynStatus(acc,accref)){
            ekf.updateAccMeasures(acc,accref);
        }else{
            ekf.updateStaticAccMeasures(acc,accref);
        }
        //ekf.updateGyroBiasMeasures(gyro,mag,tstart,att_dt);
        ekf.fuseErr2Qhat();
        ekf.resetBias();
        //ekf.updateMagMeasures(mag);
        ekf.computeAngles(rpy, rpy_align);
        PIDtick.loop(); 
        
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}