Diff: run.cpp

Revision:

120:9f4725deb5a6

Parent:

118:97ffe77b6f38

Child:

121:1d5b3e1f0d21

diff -r 43ac44c68ff0 -r 9f4725deb5a6 run.cpp
--- a/run.cpp	Thu Mar 31 01:08:44 2022 +0000
+++ b/run.cpp	Thu Mar 31 03:33:28 2022 +0000
@@ -42,7 +42,8 @@
     tail.Subscribe(tail_address[pos_tail], &(updateValues));
     pc.Subscribe(0000,&(rp));
     
-    preflightCheck();
+    if(hilFlag == false){preflightCheck();};
+
     
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
@@ -51,7 +52,12 @@
     {
         tstart = _t.read();
         //センサの値を取得
-        getIMUval();
+        if(hilFlag==false){
+            getIMUval();
+        }else{
+            getHilIMUval();
+        }
+        
         calcOpticalVel();
         
         //ekfの更新
@@ -66,10 +72,19 @@
         }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;
-                tgps = _t.read();
+            if(hilFlag==false){
+                if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
+                    getGPSval();
+                    gpsUpdateFlag = true;
+                    gpsitow = updateValues.gps_itow;
+                    tgps = _t.read();
+                }
+            }else{
+                if(tstart-tgps>0.2f){
+                    getHilGPSval();
+                    tgps = _t.read();
+                    gpsUpdateFlag = true;
+                }
             }
             
             sensorUpdateFlag = false;
@@ -79,8 +94,6 @@
             }
             
             if(gpsUpdateFlag==true){
-                Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
-                Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
                 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);