Diff: run.cpp

Revision:

100:473d550dfbfa

Parent:

98:39e4d1844a24

Child:

101:d9895b8eb49f

--- a/run.cpp	Wed Feb 09 09:40:03 2022 +0000
+++ b/run.cpp	Thu Feb 17 08:21:59 2022 +0000
@@ -38,6 +38,8 @@
     for(int i = 0;i<6;i++){
         agoffset[i] /= float(n_init);
     }
+    //eskf.setAccBias(agoffset[0],agoffset[1],agoffset[2]);
+    //eskf.setGyroBias(agoffset[3],agoffset[4],agoffset[5]);
     magref.x /= float(n_init);
     magref.y /= float(n_init);
     magref.z /= float(n_init);
@@ -64,11 +66,11 @@
     
     //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
-    eskf.setPhatPosition(0.1f);
-    eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(0.5f);
-    eskf.setPhatAccBias(0.001f);
-    eskf.setPhatGyroBias(0.001f);
+    eskf.setPhatPosition(11.0f);
+    eskf.setPhatVelocity(1.0f);
+    eskf.setPhatAngleError(1.0f);
+    eskf.setPhatAccBias(1.0f);
+    eskf.setPhatGyroBias(1.0f);
     eskf.setPhatGravity(0.0000001f);
     
     eskf.setQVelocity(0.001f);
@@ -104,8 +106,27 @@
     tail.Subscribe(tail_address[pos_tail], &(updateValues));
     
     //pre-flight check
-    
+    int i = 1;
     while(1){
+        float tstart = _t.read();
+        getIMUval();
+        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+        Matrix Reye(3,3);
+        setDiag(Reye,1.0f);
+        Matrix zeroMat(3,1);
+        zeroMat(1,1) = 0.0f;
+        zeroMat(2,1) = 0.0f;
+        zeroMat(3,1) = 0.0f;
+        if(i%3==0){
+            eskf.updateGPSVelocity(zeroMat,Reye);
+        }else if(i%3==1){
+            float heading = atan2f(-mag.y,mag.x);
+            eskf.updateHeading(heading,Rheading);
+        }else{
+            eskf.updateAcc(MatrixMath::Vector2mat(acc),Reye);
+        }
+        
         bool preflightCheck = true;
         if(sbus.failSafe){
             pc.printf("sbus\r\n");
@@ -116,13 +137,16 @@
             preflightCheck = false;
         }
         
-        if(preflightCheck == true){
+        if(preflightCheck == true && i>500){
             prefligt_finished = true;
             break;
         }
         wait(0.01f);
         pc.printf("pre-flight check\r\n");
         send2center();
+        float tend = _t.read();
+        att_dt = (tend-tstart);
+        i++;
     }
     
     wait(2.0f);