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@143:53808e4e684c, 2022-06-24 (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Jun 24 05:44:34 2022 +0000
- Revision:
- 143:53808e4e684c
- Parent:
- 140:53dbdb207542
complete
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 140:53dbdb207542 | 1 | #include "global.hpp" |
cocorlow | 140:53dbdb207542 | 2 | |
cocorlow | 140:53dbdb207542 | 3 | void getIMUval() |
cocorlow | 140:53dbdb207542 | 4 | { |
cocorlow | 140:53dbdb207542 | 5 | lsm.readAccel(); |
cocorlow | 140:53dbdb207542 | 6 | lsm.readMag(); |
cocorlow | 140:53dbdb207542 | 7 | lsm.readGyro(); |
cocorlow | 140:53dbdb207542 | 8 | |
cocorlow | 140:53dbdb207542 | 9 | Vector3f accmat; |
cocorlow | 140:53dbdb207542 | 10 | accmat << lsm.ax * 9.8f - agoffset[0], lsm.ay * 9.8f - agoffset[1], lsm.az * 9.8f - agoffset[2]; |
cocorlow | 140:53dbdb207542 | 11 | Vector3f accAlign = SensorAlignmentAG*accmat; |
cocorlow | 140:53dbdb207542 | 12 | |
cocorlow | 140:53dbdb207542 | 13 | acc = accAlign; |
cocorlow | 140:53dbdb207542 | 14 | |
cocorlow | 140:53dbdb207542 | 15 | Vector3f gyromat; |
cocorlow | 140:53dbdb207542 | 16 | gyromat << (lsm.gx * M_PI_F / 180.0f) - agoffset[3], (lsm.gy * M_PI_F / 180.0f) - agoffset[4], (lsm.gz * M_PI_F / 180.0f) - agoffset[5]; |
cocorlow | 140:53dbdb207542 | 17 | Vector3f gyroAlign = SensorAlignmentAG*gyromat; |
cocorlow | 140:53dbdb207542 | 18 | gyro = gyroAlign; |
cocorlow | 140:53dbdb207542 | 19 | |
cocorlow | 140:53dbdb207542 | 20 | Vector3f magraw; |
cocorlow | 140:53dbdb207542 | 21 | magraw << lsm.mx, lsm.my, lsm.mz; |
cocorlow | 140:53dbdb207542 | 22 | magraw = SensorAlignmentMAG*magraw; |
cocorlow | 140:53dbdb207542 | 23 | float inputMag[3]; |
cocorlow | 140:53dbdb207542 | 24 | float outputMag[3]; |
cocorlow | 140:53dbdb207542 | 25 | inputMag[0] = magraw(0)*1000.0f; |
cocorlow | 140:53dbdb207542 | 26 | inputMag[1] = magraw(1)*1000.0f; |
cocorlow | 140:53dbdb207542 | 27 | inputMag[2] = magraw(2)*1000.0f; |
cocorlow | 140:53dbdb207542 | 28 | magCalibrator.run(inputMag,outputMag); |
cocorlow | 140:53dbdb207542 | 29 | mag(0) = outputMag[0]; |
cocorlow | 140:53dbdb207542 | 30 | mag(1) = outputMag[1]; |
cocorlow | 140:53dbdb207542 | 31 | mag(2) = outputMag[2]; |
cocorlow | 140:53dbdb207542 | 32 | //twelite.printf("%f %f %f : %f %f %f\r\n",magraw(1,1),magraw(2,1),magraw(3,1),magmod(1,1),magmod(2,1),magmod(3,1)); |
cocorlow | 140:53dbdb207542 | 33 | |
cocorlow | 140:53dbdb207542 | 34 | palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0); |
cocorlow | 140:53dbdb207542 | 35 | |
cocorlow | 140:53dbdb207542 | 36 | //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); |
cocorlow | 140:53dbdb207542 | 37 | //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz); |
cocorlow | 140:53dbdb207542 | 38 | //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz); |
cocorlow | 140:53dbdb207542 | 39 | //float pressure = lps.readPressureMillibars(); |
cocorlow | 140:53dbdb207542 | 40 | //float altitude = lps.pressureToAltitudeMeters(pressure); |
cocorlow | 140:53dbdb207542 | 41 | //float temperature = lps.readTemperatureC(); |
cocorlow | 140:53dbdb207542 | 42 | //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature); |
cocorlow | 140:53dbdb207542 | 43 | } |