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