solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: imu.cpp
- Revision:
- 56:888379912f81
- Child:
- 61:c05353850017
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/imu.cpp Mon May 31 18:59:36 2021 +0000 @@ -0,0 +1,36 @@ +#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 + magval[0] = (mdata.x - magbias[0]); + magval[1] = (mdata.y - magbias[1]); + magval[2] = (mdata.z - magbias[2]); + mag.x = -magval[0]/magbias[3]; + mag.y = -magval[1]/magbias[3]; + mag.z = -magval[2]/magbias[3]; + + float lpc_acc = 0.15f; + LPacc = lpc_acc*acc + (1.0f-lpc_acc)*LPacc; + + float lpc_mag = 0.15f; + LPmag = lpc_mag*mag + (1.0f-lpc_mag)*LPmag; + + magref = ekf.calcMagRef(LPmag/LPmag.Norm()); +} \ No newline at end of file