Diff: imu.cpp

Revision:

119:a21e283730d1

Parent:

113:3e47d9881529

Child:

139:b378528c05f2

--- 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);