HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

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

imu.cpp

Committer:
NaotoMorita
Date:
2021-06-22
Revision:
66:e5afad70fdd8
Parent:
61:c05353850017
Child:
68:b9f6938fab9d

File content as of revision 66:e5afad70fdd8:

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