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
- Committer:
- NaotoMorita
- Date:
- 2021-11-08
- Revision:
- 102:1c77ff6e2a85
- Parent:
- 101:200e98dad7f8
- Child:
- 104:20b8caa29185
File content as of revision 102:1c77ff6e2a85:
#include "global.hpp" void getIMUval() { lsm.readAccel(); lsm.readMag(); lsm.readGyro(); Matrix accmat(3,1); accmat << lsm.ax * 9.8f - agoffset[0] << lsm.ay * 9.8f - agoffset[1] << lsm.az * 9.8f - agoffset[2]; Matrix accAlign = SensorAlignment*accmat; acc.x = accAlign(1,1); acc.y = accAlign(2,1); acc.z = accAlign(3,1); Matrix gyromat(3,1); 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]; Matrix gyroAlign = SensorAlignment*gyromat; gyro.x = gyroAlign(1,1); gyro.y = gyroAlign(2,1); gyro.z = gyroAlign(3,1); Matrix magraw(3,1); magraw << lsm.mx <<lsm.my << lsm.mz; magres = magCalibrator.calcResidial(magraw); if(magres > magresThreshold){ magCalibrator.updateParams(magraw,0.001f); }; Matrix magmod = SensorAlignment*magCalibrator.outCalibratedMag(magraw); mag.x = magmod(1,1); mag.y = magmod(2,1); mag.z = magmod(3,1); palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0); //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); //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz); //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz); //float pressure = lps.readPressureMillibars(); //float altitude = lps.pressureToAltitudeMeters(pressure); //float temperature = lps.readTemperatureC(); //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature); }