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@104:20b8caa29185, 2021-11-10 (annotated)
- Committer:
- NaotoMorita
- Date:
- Wed Nov 10 06:34:16 2021 +0000
- Revision:
- 104:20b8caa29185
- Parent:
- 102:1c77ff6e2a85
- Child:
- 111:0fae4fbe2a80
mag implemented
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 | { |
osaka | 88:be349faa1976 | 5 | lsm.readAccel(); |
NaotoMorita | 89:c9f64bd655d9 | 6 | lsm.readMag(); |
osaka | 88:be349faa1976 | 7 | lsm.readGyro(); |
NaotoMorita | 92:00460f6df439 | 8 | |
NaotoMorita | 93:b827f78a717a | 9 | Matrix accmat(3,1); |
NaotoMorita | 93:b827f78a717a | 10 | accmat << lsm.ax * 9.8f - agoffset[0] << lsm.ay * 9.8f - agoffset[1] << lsm.az * 9.8f - agoffset[2]; |
NaotoMorita | 93:b827f78a717a | 11 | Matrix accAlign = SensorAlignment*accmat; |
NaotoMorita | 93:b827f78a717a | 12 | |
NaotoMorita | 93:b827f78a717a | 13 | acc.x = accAlign(1,1); |
NaotoMorita | 93:b827f78a717a | 14 | acc.y = accAlign(2,1); |
NaotoMorita | 93:b827f78a717a | 15 | acc.z = accAlign(3,1); |
NaotoMorita | 93:b827f78a717a | 16 | |
NaotoMorita | 93:b827f78a717a | 17 | Matrix gyromat(3,1); |
NaotoMorita | 93:b827f78a717a | 18 | gyromat << (lsm.gx * M_PI / 180.0f) - agoffset[3] << (lsm.gy * M_PI / 180.0f) - agoffset[4] << (lsm.gz * M_PI / 180.0f) - agoffset[5]; |
NaotoMorita | 93:b827f78a717a | 19 | Matrix gyroAlign = SensorAlignment*gyromat; |
NaotoMorita | 93:b827f78a717a | 20 | gyro.x = gyroAlign(1,1); |
NaotoMorita | 93:b827f78a717a | 21 | gyro.y = gyroAlign(2,1); |
NaotoMorita | 93:b827f78a717a | 22 | gyro.z = gyroAlign(3,1); |
NaotoMorita | 92:00460f6df439 | 23 | |
NaotoMorita | 101:200e98dad7f8 | 24 | Matrix magraw(3,1); |
NaotoMorita | 101:200e98dad7f8 | 25 | magraw << lsm.mx <<lsm.my << lsm.mz; |
NaotoMorita | 102:1c77ff6e2a85 | 26 | magres = magCalibrator.calcResidial(magraw); |
NaotoMorita | 102:1c77ff6e2a85 | 27 | if(magres > magresThreshold){ |
NaotoMorita | 104:20b8caa29185 | 28 | magCalibrator.updateParams(magraw,0.1f); |
NaotoMorita | 101:200e98dad7f8 | 29 | }; |
NaotoMorita | 102:1c77ff6e2a85 | 30 | Matrix magmod = SensorAlignment*magCalibrator.outCalibratedMag(magraw); |
NaotoMorita | 101:200e98dad7f8 | 31 | mag.x = magmod(1,1); |
NaotoMorita | 101:200e98dad7f8 | 32 | mag.y = magmod(2,1); |
NaotoMorita | 101:200e98dad7f8 | 33 | mag.z = magmod(3,1); |
NaotoMorita | 101:200e98dad7f8 | 34 | |
NaotoMorita | 99:98b892ca70ec | 35 | palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0); |
NaotoMorita | 92:00460f6df439 | 36 | |
osaka | 88:be349faa1976 | 37 | //printf("%f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz); |
osaka | 88:be349faa1976 | 38 | //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz); |
osaka | 88:be349faa1976 | 39 | //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz); |
osaka | 88:be349faa1976 | 40 | //float pressure = lps.readPressureMillibars(); |
osaka | 88:be349faa1976 | 41 | //float altitude = lps.pressureToAltitudeMeters(pressure); |
osaka | 88:be349faa1976 | 42 | //float temperature = lps.readTemperatureC(); |
NaotoMorita | 92:00460f6df439 | 43 | //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature); |
NaotoMorita | 68:b9f6938fab9d | 44 | } |