Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: imu.cpp
- Revision:
- 101:200e98dad7f8
- Parent:
- 99:98b892ca70ec
- Child:
- 102:1c77ff6e2a85
--- a/imu.cpp Fri Nov 05 13:48:22 2021 +0000 +++ b/imu.cpp Sun Nov 07 05:38:17 2021 +0000 @@ -21,6 +21,17 @@ gyro.y = gyroAlign(2,1); gyro.z = gyroAlign(3,1); + Matrix magraw(3,1); + magraw << lsm.mx <<lsm.my << lsm.mz; + float magres = magCalibrator.calcResidial(magraw); + if(magres>0.01){ + magCalibrator.updateParams(magraw,0.0001f); + }; + Matrix magmod = 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);