Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

imu.cpp

Committer:
NaotoMorita
Date:
2021-06-28
Revision:
68:b9f6938fab9d
Parent:
66:e5afad70fdd8
Child:
70:99f974d8960e

File content as of revision 68:b9f6938fab9d:

#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 = 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];
}