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-03
- Revision:
- 61:c05353850017
- Parent:
- 56:888379912f81
- Child:
- 66:e5afad70fdd8
File content as of revision 61:c05353850017:
#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]; float lpc_acc = 0.95f; LPacc = lpc_acc*acc + (1.0f-lpc_acc)*LPacc; float lpc_mag = 0.45f; LPmag = lpc_mag*mag + (1.0f-lpc_mag)*LPmag; val_thmg = 0.99f*val_thmg+0.01f*acos((mag % acc)/mag.Norm()/acc.Norm()); accref.z = 0.99f*accref.z + 0.01f*(-acc.Norm()); magref = ekf.calcMagRef(LPmag/LPmag.Norm()); }