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@76:7fd3ac1afe3e, 2021-08-07 (annotated)
- Committer:
- NaotoMorita
- Date:
- Sat Aug 07 06:54:58 2021 +0000
- Revision:
- 76:7fd3ac1afe3e
- Parent:
- 74:f67062e7813e
- Child:
- 87:89bbbcdb667b
async update
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 |
NaotoMorita | 76:7fd3ac1afe3e | 6 | //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
NaotoMorita | 76:7fd3ac1afe3e | 7 | ax = accelgyro.getAccelerationX() - agoffset[0]; |
NaotoMorita | 76:7fd3ac1afe3e | 8 | ay = accelgyro.getAccelerationY() - agoffset[1]; |
NaotoMorita | 76:7fd3ac1afe3e | 9 | az = accelgyro.getAccelerationZ() - agoffset[2]; |
NaotoMorita | 76:7fd3ac1afe3e | 10 | gx = accelgyro.getRotationX() - agoffset[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 11 | gy = accelgyro.getRotationY() - agoffset[4]; |
NaotoMorita | 76:7fd3ac1afe3e | 12 | gz = accelgyro.getRotationZ() - agoffset[5]; |
cocorlow | 56:888379912f81 | 13 | // 加速度値を分解能で割って加速度(G)に変換する |
NaotoMorita | 70:99f974d8960e | 14 | acc.x = mapfloat(float(ax) / ACCEL_SSF,accMin[0],accMax[0],-1.0f,1.0f); //FS_SEL_0 16,384 LSB / g |
NaotoMorita | 70:99f974d8960e | 15 | acc.y = mapfloat(float(ay) / ACCEL_SSF,accMin[1],accMax[1],-1.0f,1.0f); |
NaotoMorita | 70:99f974d8960e | 16 | acc.z = mapfloat(float(az) / ACCEL_SSF,accMin[2],accMax[2],-1.0f,1.0f); |
NaotoMorita | 70:99f974d8960e | 17 | acc = accMedian.Process(acc); |
cocorlow | 56:888379912f81 | 18 | // 角速度値を分解能で割って角速度(rad per sec)に変換する |
cocorlow | 56:888379912f81 | 19 | gyro.x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s) |
cocorlow | 56:888379912f81 | 20 | gyro.y = float(gy) / GYRO_SSF * 0.0174533f; |
cocorlow | 56:888379912f81 | 21 | gyro.z = float(gz) / GYRO_SSF * 0.0174533f; |
NaotoMorita | 70:99f974d8960e | 22 | gyro = gyroMedian.Process(gyro); |
cocorlow | 56:888379912f81 | 23 | mag_sensor.getAxis(mdata); // flush the magnetmeter |
NaotoMorita | 76:7fd3ac1afe3e | 24 | float magval[3] = {0.0f, 0.0f, 0.0f}; |
NaotoMorita | 76:7fd3ac1afe3e | 25 | magval[0] = (mdata.x - magbias[0]); |
NaotoMorita | 76:7fd3ac1afe3e | 26 | magval[1] = (mdata.y - magbias[1]); |
NaotoMorita | 76:7fd3ac1afe3e | 27 | magval[2] = (mdata.z - magbias[2]); |
NaotoMorita | 76:7fd3ac1afe3e | 28 | mag.x = -magval[1]/magbias[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 29 | mag.y = -magval[0]/magbias[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 30 | mag.z = magval[2]/magbias[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 31 | /* |
NaotoMorita | 61:c05353850017 | 32 | float inputMag[3]; |
NaotoMorita | 61:c05353850017 | 33 | float outputMag[3]; |
NaotoMorita | 61:c05353850017 | 34 | inputMag[0] = mdata.x; |
NaotoMorita | 61:c05353850017 | 35 | inputMag[1] = mdata.y; |
NaotoMorita | 61:c05353850017 | 36 | inputMag[2] = mdata.z; |
NaotoMorita | 61:c05353850017 | 37 | magCalibrator.run(inputMag,outputMag); |
NaotoMorita | 74:f67062e7813e | 38 | mag.x = -outputMag[1]; |
NaotoMorita | 74:f67062e7813e | 39 | mag.y = -outputMag[0]; |
NaotoMorita | 74:f67062e7813e | 40 | mag.z = outputMag[2]; |
NaotoMorita | 76:7fd3ac1afe3e | 41 | */ |
NaotoMorita | 70:99f974d8960e | 42 | mag = magMedian.Process(mag); |
NaotoMorita | 68:b9f6938fab9d | 43 | } |