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@56:888379912f81, 2021-05-31 (annotated)
- Committer:
- cocorlow
- Date:
- Mon May 31 18:59:36 2021 +0000
- Revision:
- 56:888379912f81
- Child:
- 61:c05353850017
file divided
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #include "global.hpp" |
cocorlow | 56:888379912f81 | 2 | |
cocorlow | 56:888379912f81 | 3 | void getIMUval() |
cocorlow | 56:888379912f81 | 4 | { |
cocorlow | 56:888379912f81 | 5 | // gx gy gz ax ay az |
cocorlow | 56:888379912f81 | 6 | accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); |
cocorlow | 56:888379912f81 | 7 | ax = ax - agoffset[0]; |
cocorlow | 56:888379912f81 | 8 | ay = ay - agoffset[1]; |
cocorlow | 56:888379912f81 | 9 | az = -az - agoffset[2]; |
cocorlow | 56:888379912f81 | 10 | gx = gx - agoffset[3]; |
cocorlow | 56:888379912f81 | 11 | gy = gy - agoffset[4]; |
cocorlow | 56:888379912f81 | 12 | gz = -gz - agoffset[5]; |
cocorlow | 56:888379912f81 | 13 | // 加速度値を分解能で割って加速度(G)に変換する |
cocorlow | 56:888379912f81 | 14 | acc.x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g |
cocorlow | 56:888379912f81 | 15 | acc.y = float(ay) / ACCEL_SSF; |
cocorlow | 56:888379912f81 | 16 | acc.z = float(az) / ACCEL_SSF; |
cocorlow | 56:888379912f81 | 17 | // 角速度値を分解能で割って角速度(rad per sec)に変換する |
cocorlow | 56:888379912f81 | 18 | gyro.x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s) |
cocorlow | 56:888379912f81 | 19 | gyro.y = float(gy) / GYRO_SSF * 0.0174533f; |
cocorlow | 56:888379912f81 | 20 | gyro.z = float(gz) / GYRO_SSF * 0.0174533f; |
cocorlow | 56:888379912f81 | 21 | mag_sensor.getAxis(mdata); // flush the magnetmeter |
cocorlow | 56:888379912f81 | 22 | magval[0] = (mdata.x - magbias[0]); |
cocorlow | 56:888379912f81 | 23 | magval[1] = (mdata.y - magbias[1]); |
cocorlow | 56:888379912f81 | 24 | magval[2] = (mdata.z - magbias[2]); |
cocorlow | 56:888379912f81 | 25 | mag.x = -magval[0]/magbias[3]; |
cocorlow | 56:888379912f81 | 26 | mag.y = -magval[1]/magbias[3]; |
cocorlow | 56:888379912f81 | 27 | mag.z = -magval[2]/magbias[3]; |
cocorlow | 56:888379912f81 | 28 | |
cocorlow | 56:888379912f81 | 29 | float lpc_acc = 0.15f; |
cocorlow | 56:888379912f81 | 30 | LPacc = lpc_acc*acc + (1.0f-lpc_acc)*LPacc; |
cocorlow | 56:888379912f81 | 31 | |
cocorlow | 56:888379912f81 | 32 | float lpc_mag = 0.15f; |
cocorlow | 56:888379912f81 | 33 | LPmag = lpc_mag*mag + (1.0f-lpc_mag)*LPmag; |
cocorlow | 56:888379912f81 | 34 | |
cocorlow | 56:888379912f81 | 35 | magref = ekf.calcMagRef(LPmag/LPmag.Norm()); |
cocorlow | 56:888379912f81 | 36 | } |