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-29
- Revision:
- 70:99f974d8960e
- Parent:
- 68:b9f6938fab9d
- Child:
- 74:f67062e7813e
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); }