solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: imu.cpp
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
--- a/imu.cpp Mon Dec 06 08:26:16 2021 +0000 +++ b/imu.cpp Mon Dec 06 11:37:55 2021 +0000 @@ -1,47 +1,43 @@ -//#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 = SensorAlignmentAG*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 = SensorAlignmentAG*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; -// magraw = SensorAlignmentMAG*magraw; -// float inputMag[3]; -// float outputMag[3]; -// inputMag[0] = magraw(1,1)*1000.0f; -// inputMag[1] = magraw(2,1)*1000.0f; -// inputMag[2] = magraw(3,1)*1000.0f; -// magCalibrator.run(inputMag,outputMag); -// mag.x = outputMag[0]; -// mag.y = outputMag[1]; -// mag.z = 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); -//} \ No newline at end of file +#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); +} \ No newline at end of file