Diff: run.cpp

Revision:

127:d73a6233ee4b

Parent:

126:03da3a8c8870

Child:

129:a76be8380bb2

--- a/run.cpp	Sun Nov 21 08:24:51 2021 +0000
+++ b/run.cpp	Mon Nov 22 09:51:51 2021 +0000
@@ -46,37 +46,24 @@
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
     
-    eskf.setQVelocity(0.0025f);
+    eskf.setQVelocity(0.000025f);
     eskf.setQAngleError(0.00025f);
     eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.000001f);
     
-    Matrix Rgps(5,5);
-    setDiag(Rgps,2.0f);
-    //Rgps(6,6) = 100.0f;
-    
-    Matrix Rgpspos(3,3);
-    setDiag(Rgpspos,2.0f);
-    
-    Matrix Rgpsvel(3,3);
-    setDiag(Rgpsvel,2.0f);
-    //Rgpsvel(3,3) = 10.0f;
-    
-    Matrix RImuConst(5,5);
-    setDiag(RImuConst,50.0f);
-    RImuConst(4,4) = 0.001f;
-    RImuConst(5,5) = 0.001f;
-    
-    Matrix RotherConst(2,2);
-    setDiag(RotherConst,0.1f);
+    Matrix Rgps(6,6);
+    setDiag(Rgps,1.0f);
+    //Rgps(4,4) = 0.1f;
+    //Rgps(5,5) = 0.1f;
     
     Matrix Racc(3,3);
     setDiag(Racc,50.0f);
-    
+
     Matrix Rheading(1,1);
-    Rheading(1,1) = 0.1f;
+    Rheading(1,1) = 0.01f;
     _t.start();
     float tgps = _t.read();
+    float theading = _t.read();
     while(1)
     {
         float tstart = _t.read();
@@ -101,13 +88,23 @@
         }else{
             getGPSval();
         }
+        
+        headingUpdateFlag = false;
+        if(tstart-theading>0.05f){
+            Matrix euler = eskf.computeAngles();
+            if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
+                headingUpdateFlag = true;
+                theading = _t.read();
+            }
+        }
+        
         if(gpsUpdateFlag == true){
             eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
-            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
-            
+            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgps);
+        }else if(headingUpdateFlag == true){
+                float heading = atan2f(-mag.y,mag.x);
+                eskf.updateHeading(heading,Rheading);
         }else{
-            eskf.updateHeading(-0.1f,Rheading);
             eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
         }
         PIDtick.loop();