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