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:
- cocorlow
- Date:
- 2021-05-31
- Revision:
- 56:888379912f81
- Child:
- 61:c05353850017
File content as of revision 56:888379912f81:
#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()); }