File content as of revision 66:e5afad70fdd8:

#include "global.hpp"

void run()
{
    getIMUval();
    ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
    float sum2accnorm = 0;
    float sumaccnorm = 0;
    for(int i = 0; i < 1000; i++){
        getIMUval();
        sumaccnorm += acc.Norm();
        sum2accnorm += acc.Norm()*acc.Norm();
    }
    accref.z = -sumaccnorm / 1000.0f;
    float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
    pc.Subscribe(0000, &(posValues));
    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    Timer _t;
    _t.start();
    
    while(1)
    {
        float tstart = _t.read();
        //姿勢角を更新
        getIMUval();
        //ekf.updateBetweenMeasures(gyro, att_dt);
        //ekf.computeAngles(rpy, rpy_g, rpy_align);
        PIDtick.loop(); 
        
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}