Diff: run.cpp

Revision:

102:1c77ff6e2a85

Parent:

100:7589b663d462

Child:

103:fec71c2051c5

--- a/run.cpp	Sun Nov 07 05:38:17 2021 +0000
+++ b/run.cpp	Mon Nov 08 09:20:21 2021 +0000
@@ -21,10 +21,20 @@
         agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
         agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
         palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
+        if(hilFlag == true){
+            magref.x += lsm.mx;
+            magref.y += lsm.my;
+            magref.z += lsm.mz;
+        }
     }
     for(int i = 0;i<6;i++){
         agoffset[i] /= float(n_init);
     }
+    if(hilFlag == true){
+        magref.x /= float(n_init);
+        magref.y /= float(n_init);
+        magref.z /= float(n_init);
+    }
     gps.Update();
     gps.CalculateUnit();
     palt0 /= float(n_init);
@@ -62,9 +72,13 @@
     
     Matrix RaccConst(4,4);
     setDiag(RaccConst,50.0f);
-    RaccConst(4,4) = 5.0f;
+    RaccConst(4,4) = 1.0f;
     //Matrix RgyroConst(2,2);
     //setDiag(RgyroConst,100);
+    Matrix Rmag(2,2);
+    Rmag(1,1) = 1.0f;
+    Rmag(2,2) = 1.0f;
+    
     _t.start();
     float tgps = _t.read();
     while(1)
@@ -96,6 +110,9 @@
             eskf.updateGPS(MatrixMath::Vector2mat(pi),MatrixMath::Vector2mat(vi),Rgps);
         }
         eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst);
+        if(magres < magresThreshold){
+            eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
+        };
         eskf.fuseErr2Nominal();
         
         PIDtick.loop();