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:
- 119:a21e283730d1
- Parent:
- 113:3e47d9881529
- Child:
- 139:b378528c05f2
diff -r f899fd694e2a -r a21e283730d1 imu.cpp --- a/imu.cpp Wed Nov 17 05:12:24 2021 +0000 +++ b/imu.cpp Thu Nov 18 08:48:29 2021 +0000 @@ -23,14 +23,16 @@ 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 = SensorAlignmentMAG*magCalibrator.outCalibratedMag(magraw); - mag.x = magmod(1,1); - mag.y = magmod(2,1); - mag.z = magmod(3,1); + 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);