File content as of revision 70:99f974d8960e:

#include "global.hpp"

void getIMUval()
{
    // gx gy gz ax ay az
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &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 = mapfloat(float(ax) / ACCEL_SSF,accMin[0],accMax[0],-1.0f,1.0f);  //FS_SEL_0 16,384 LSB / g
    acc.y = mapfloat(float(ay) / ACCEL_SSF,accMin[1],accMax[1],-1.0f,1.0f);
    acc.z = mapfloat(float(az) / ACCEL_SSF,accMin[2],accMax[2],-1.0f,1.0f);
    acc = accMedian.Process(acc);
    // 角速度値を分解能で割って角速度(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;
    gyro = gyroMedian.Process(gyro);
    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];
    mag = magMedian.Process(mag);
}