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:
- cocorlow
- Date:
- 2021-12-06
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
File content as of revision 140:53dbdb207542:
#include "global.hpp" void getIMUval() { lsm.readAccel(); lsm.readMag(); lsm.readGyro(); Vector3f accmat; accmat << lsm.ax * 9.8f - agoffset[0], lsm.ay * 9.8f - agoffset[1], lsm.az * 9.8f - agoffset[2]; Vector3f accAlign = SensorAlignmentAG*accmat; acc = accAlign; Vector3f gyromat; 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]; Vector3f gyroAlign = SensorAlignmentAG*gyromat; gyro = gyroAlign; Vector3f magraw; magraw << lsm.mx, lsm.my, lsm.mz; magraw = SensorAlignmentMAG*magraw; float inputMag[3]; float outputMag[3]; inputMag[0] = magraw(0)*1000.0f; inputMag[1] = magraw(1)*1000.0f; inputMag[2] = magraw(2)*1000.0f; magCalibrator.run(inputMag,outputMag); mag(0) = outputMag[0]; mag(1) = outputMag[1]; mag(2) = outputMag[2]; //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)); 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); }