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@66:e5afad70fdd8, 2021-06-22 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Jun 22 02:19:14 2021 +0000
- Revision:
- 66:e5afad70fdd8
- Parent:
- 61:c05353850017
- Child:
- 68:b9f6938fab9d
mbed ram start;
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 |
NaotoMorita | 61:c05353850017 | 22 | float inputMag[3]; |
NaotoMorita | 61:c05353850017 | 23 | float outputMag[3]; |
NaotoMorita | 61:c05353850017 | 24 | inputMag[0] = mdata.x; |
NaotoMorita | 61:c05353850017 | 25 | inputMag[1] = mdata.y; |
NaotoMorita | 61:c05353850017 | 26 | inputMag[2] = mdata.z; |
NaotoMorita | 61:c05353850017 | 27 | magCalibrator.run(inputMag,outputMag); |
NaotoMorita | 61:c05353850017 | 28 | mag.x = -outputMag[0]; |
NaotoMorita | 61:c05353850017 | 29 | mag.y = -outputMag[1]; |
NaotoMorita | 61:c05353850017 | 30 | mag.z = -outputMag[2]; |
cocorlow | 56:888379912f81 | 31 | } |