HAPSRG / Mbed 2 deprecated hexaTest_Eigen

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@119:a21e283730d1, 2021-11-18 (annotated)

Committer:
NaotoMorita
Date:
Thu Nov 18 08:48:29 2021 +0000
Revision:
119:a21e283730d1
Parent:
113:3e47d9881529
Child:
139:b378528c05f2
no mag bias

Who changed what in which revision?

UserRevisionLine numberNew 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 113:3e47d9881529 11 Matrix accAlign = SensorAlignmentAG*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 113:3e47d9881529 19 Matrix gyroAlign = SensorAlignmentAG*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 119:a21e283730d1 26 magraw = SensorAlignmentMAG*magraw;
NaotoMorita 119:a21e283730d1 27 float inputMag[3];
NaotoMorita 119:a21e283730d1 28 float outputMag[3];
NaotoMorita 119:a21e283730d1 29 inputMag[0] = magraw(1,1)*1000.0f;
NaotoMorita 119:a21e283730d1 30 inputMag[1] = magraw(2,1)*1000.0f;
NaotoMorita 119:a21e283730d1 31 inputMag[2] = magraw(3,1)*1000.0f;
NaotoMorita 119:a21e283730d1 32 magCalibrator.run(inputMag,outputMag);
NaotoMorita 119:a21e283730d1 33 mag.x = outputMag[0];
NaotoMorita 119:a21e283730d1 34 mag.y = outputMag[1];
NaotoMorita 119:a21e283730d1 35 mag.z = outputMag[2];
NaotoMorita 113:3e47d9881529 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));
NaotoMorita 101:200e98dad7f8 37
NaotoMorita 99:98b892ca70ec 38 palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
NaotoMorita 92:00460f6df439 39
osaka 88:be349faa1976 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);
osaka 88:be349faa1976 41 //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
osaka 88:be349faa1976 42 //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
osaka 88:be349faa1976 43 //float pressure = lps.readPressureMillibars();
osaka 88:be349faa1976 44 //float altitude = lps.pressureToAltitudeMeters(pressure);
osaka 88:be349faa1976 45 //float temperature = lps.readTemperatureC();
NaotoMorita 92:00460f6df439 46 //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
NaotoMorita 68:b9f6938fab9d 47 }