Diff: run.cpp

Revision:

101:d9895b8eb49f

Parent:

100:473d550dfbfa

Child:

103:34e911b94440

--- a/run.cpp	Thu Feb 17 08:21:59 2022 +0000
+++ b/run.cpp	Fri Feb 18 11:49:52 2022 +0000
@@ -66,13 +66,13 @@
     
     //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
-    eskf.setPhatPosition(11.0f);
-    eskf.setPhatVelocity(1.0f);
-    eskf.setPhatAngleError(1.0f);
-    eskf.setPhatAccBias(1.0f);
-    eskf.setPhatGyroBias(1.0f);
+    eskf.setPhatPosition(0.1f);
+    eskf.setPhatVelocity(0.1f);
+    eskf.setPhatAngleError(0.5f);
+    eskf.setPhatAccBias(0.001f);
+    eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
-    
+
     eskf.setQVelocity(0.001f);
     eskf.setQAngleError(0.0000025f);
     eskf.setQAccBias(0.000001f);
@@ -108,24 +108,24 @@
     //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);
-        }
+        //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){
@@ -136,6 +136,7 @@
             pc.printf("gps\r\n");
             preflightCheck = false;
         }
+        //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );
         
         if(preflightCheck == true && i>500){
             prefligt_finished = true;
@@ -144,8 +145,8 @@
         wait(0.01f);
         pc.printf("pre-flight check\r\n");
         send2center();
-        float tend = _t.read();
-        att_dt = (tend-tstart);
+        //float tend = _t.read();
+        //att_dt = (tend-tstart);
         i++;
     }
     
@@ -193,6 +194,7 @@
             }
         }else{
             gpsUpdateFlag = false;
+            //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );
             if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
                 gpsUpdateFlag = true;
                 gpsitow = updateValues.gps_itow;
@@ -214,6 +216,7 @@
                 Rgps(1, 1) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
                 Rgps(2, 2) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
                 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+                pc.printf("gps\r\n");
             }else if(headingUpdateFlag == true){
                     float heading = atan2f(-mag.y,mag.x);
                     eskf.updateHeading(heading,Rheading);