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