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:
- 113:3e47d9881529
- Parent:
- 111:0fae4fbe2a80
- Child:
- 114:88abd741f44d
- Child:
- 119:a21e283730d1
--- a/imu.cpp Tue Nov 16 01:21:01 2021 +0000 +++ b/imu.cpp Tue Nov 16 13:57:32 2021 +0000 @@ -8,7 +8,7 @@ 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; + Matrix accAlign = SensorAlignmentAG*accmat; acc.x = accAlign(1,1); acc.y = accAlign(2,1); @@ -16,21 +16,22 @@ 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; + 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; - //magres = magCalibrator.calcResidial(magraw); - //if(magres > magresThreshold){ - // magCalibrator.updateParams(magraw,0.1f); - //}; - Matrix magmod = SensorAlignment*magraw; + magres = magCalibrator.calcResidial(magraw); + if(magres > magresThreshold){ + magCalibrator.updateParams(magraw,0.1f); + }; + Matrix magmod = SensorAlignmentMAG*magCalibrator.outCalibratedMag(magraw); mag.x = magmod(1,1); mag.y = magmod(2,1); mag.z = magmod(3,1); + //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);