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@114:88abd741f44d, 2021-11-16 (annotated)

Committer:
NaotoMorita
Date:
Tue Nov 16 14:17:57 2021 +0000
Revision:
114:88abd741f44d
Parent:
113:3e47d9881529 
mag Radius Estimate

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 114:88abd741f44d 26 //magres = magCalibrator.calcResidial(magraw);
NaotoMorita 114:88abd741f44d 27 //if(magres > magresThreshold){
NaotoMorita 114:88abd741f44d 28 // magCalibrator.updateParams(magraw,0.1f);
NaotoMorita 114:88abd741f44d 29 //};
NaotoMorita 114:88abd741f44d 30 Matrix magmod = SensorAlignmentMAG*(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 113:3e47d9881529 34 //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 35
NaotoMorita 99:98b892ca70ec 36 palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
NaotoMorita 92:00460f6df439 37
osaka 88:be349faa1976 38 //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 39 //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
osaka 88:be349faa1976 40 //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
osaka 88:be349faa1976 41 //float pressure = lps.readPressureMillibars();
osaka 88:be349faa1976 42 //float altitude = lps.pressureToAltitudeMeters(pressure);
osaka 88:be349faa1976 43 //float temperature = lps.readTemperatureC();
NaotoMorita 92:00460f6df439 44 //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
NaotoMorita 68:b9f6938fab9d 45 }