File content as of revision 61:c05353850017:

#include "global.hpp"

void getIMUval()
{
    // gx gy gz ax ay az
    accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
    ax =  ax - agoffset[0];
    ay =  ay - agoffset[1];
    az =  -az - agoffset[2];
    gx = gx - agoffset[3];
    gy = gy - agoffset[4];
    gz = -gz - agoffset[5];
    // 加速度値を分解能で割って加速度(G)に変換する
    acc.x = float(ax) / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
    acc.y = float(ay) / ACCEL_SSF;
    acc.z = float(az) / ACCEL_SSF;
    // 角速度値を分解能で割って角速度(rad per sec)に変換する
    gyro.x = float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
    gyro.y = float(gy) / GYRO_SSF * 0.0174533f;
    gyro.z = float(gz) / GYRO_SSF * 0.0174533f;
    mag_sensor.getAxis(mdata); // flush the magnetmeter
    float inputMag[3];
    float outputMag[3];
    inputMag[0] = mdata.x;
    inputMag[1] = mdata.y;
    inputMag[2] = mdata.z;
    magCalibrator.run(inputMag,outputMag);
    mag.x = -outputMag[0];
    mag.y = -outputMag[1];
    mag.z = -outputMag[2];
    
    float lpc_acc = 0.95f;
    LPacc = lpc_acc*acc + (1.0f-lpc_acc)*LPacc;

    float lpc_mag = 0.45f;
    LPmag = lpc_mag*mag + (1.0f-lpc_mag)*LPmag;
    
    val_thmg = 0.99f*val_thmg+0.01f*acos((mag % acc)/mag.Norm()/acc.Norm());
    accref.z = 0.99f*accref.z + 0.01f*(-acc.Norm());
    
    magref = ekf.calcMagRef(LPmag/LPmag.Norm());
}