File content as of revision 70:9e7be21475f8:

#include "global.hpp"

void run()
{
    pc.printf("reading calibration value\r\n");
    //キャリブレーション値を取得
    U read_calib;
    readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
    wait(3);
    pos_tail = (int)read_calib.i[0];
    agoffset[3] = float(read_calib.i[7]);
    agoffset[4] = float(read_calib.i[8]);
    agoffset[5] = float(read_calib.i[9]);
    magbiasMin[0] = float(read_calib.i[1])/1000.0f;
    magbiasMin[1] = float(read_calib.i[2])/1000.0f;
    magbiasMin[2] = float(read_calib.i[3])/1000.0f;
    magbiasMax[0] = float(read_calib.i[4])/1000.0f;
    magbiasMax[1] = float(read_calib.i[5])/1000.0f;
    magbiasMax[2] = float(read_calib.i[6])/1000.0f;
    rpy_align.y = float(read_calib.i[10])/200000.0f;
    rpy_align.x = float(read_calib.i[11])/200000.0f;
    accMin[0] = float(read_calib.i[12])/1000.0f;
    accMin[1] = float(read_calib.i[13])/1000.0f;
    accMin[2] = float(read_calib.i[14])/1000.0f;
    accMax[0] = float(read_calib.i[15])/1000.0f;
    accMax[1] = float(read_calib.i[16])/1000.0f;
    accMax[2] = float(read_calib.i[17])/1000.0f;
    magCalibrator.setExtremes(magbiasMin,magbiasMax);
//    tail_address[pos_tail] = (int)read_calib.i[10];
    
    switch(pos_tail){
    case 0:
        pc.printf("This MBED is Located at Left \r\n");
        break;
    case 1:
        pc.printf("This MBED is Located at Center \r\n");
        break;
    case 2:
        pc.printf("This MBED is Located at Right \r\n");
        break;
    default:   // error situation
        pc.printf("error\r\n");
        break;
    }
    pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
    pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.y*180.0f/M_PI,rpy_align.x*180.0f/M_PI);
    getIMUval();
    //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
    ekf.defineQhat(rpy_align);
    float sum2accnorm = 0;
    float sumaccnorm = 0;
    
    Timer _t;
    _t.start();
    
    for(int i = 0; i < 1000; i++){
        float tstart = _t.read();
        getIMUval();
        ekf.updateQhat(gyro, att_dt);
        ekf.updateErrState(gyro, att_dt);
        ekf.updateStaticAccMeasures(acc,accref);
        ekf.fuseErr2Qhat();
        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;
    for (int i = 0; i < 3; i++)
    {
        if (i == pos_tail)
        {
            continue;
        }
        else
        {
            tail.Subscribe(tail_address[i], &(posValues[i]));
        }
    }
    tail.Subscribe(time_address, &broadcast_time);
    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    
    while(1)
    {
        float tstart = _t.read();
        //姿勢角を更新
        getIMUval();
        ekf.updateQhat(gyro, att_dt);
        ekf.updateErrState(gyro, att_dt);
        if(ekf.determinDynStatus(acc,accref)){
            ekf.updateAccMeasures(acc,accref);
        }else{
            ekf.updateStaticAccMeasures(acc,accref);
        }
        ekf.fuseErr2Qhat();
        ekf.resetBias();
        ekf.updateMagMeasures(mag);
        ekf.computeAngles(rpy, rpy_align);
        PIDtick.loop(); 
        
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}