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:
- 139:b378528c05f2
- Parent:
- 119:a21e283730d1
- Child:
- 140:53dbdb207542
--- a/imu.cpp Wed Dec 01 19:17:36 2021 +0000 +++ b/imu.cpp Mon Dec 06 08:26:16 2021 +0000 @@ -1,47 +1,47 @@ -#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(); +// +// 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