Diff: run.cpp

Revision:

95:98dbbcc6b39d

Parent:

94:a90b4b920fa9

Child:

98:39e4d1844a24

--- a/run.cpp	Tue Feb 01 15:15:46 2022 +0000
+++ b/run.cpp	Wed Feb 02 07:59:58 2022 +0000
@@ -104,42 +104,9 @@
     tail.Subscribe(tail_address[pos_tail], &(updateValues));
     
     //pre-flight check
-    int i = 0;
+    
     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);
-        Vector3 pi(0.0f, 0.0f, 0.0f);
-        Vector3 vi(0.0f, 0.0f, 0.0f);
-        eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-        float heading = atan2f(-mag.y,mag.x);
-        eskf.updateHeading(heading,Rheading);
-        Matrix Raccpreflight(3,3);
-        setDiag(Raccpreflight,5.0f);
-        eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
-        
         bool preflightCheck = true;
-        Matrix gyroBias = eskf.getGyroBias();
-        if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
-            pc.printf("gyro\r\n");
-            preflightCheck = false;
-        }
-        Matrix accBias = eskf.getAccBias();
-        if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
-            pc.printf("acc\r\n");
-            preflightCheck = false;
-        }
-        Matrix vihat = eskf.getVihat();
-        if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
-            pc.printf("vi\r\n");
-            preflightCheck = false;
-        }
-        Matrix pihat = eskf.getPihat();
-        if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
-            pc.printf("pi\r\n");
-            preflightCheck = false;
-        }
         if(sbus.failSafe){
             pc.printf("sbus\r\n");
             preflightCheck = false;
@@ -149,18 +116,16 @@
             preflightCheck = false;
         }
         
-        if(preflightCheck == true && i >100){
+        if(preflightCheck == true){
             prefligt_finished = true;
             break;
         }
         wait(0.01f);
-        float tend = _t.read();
-        att_dt = (tend-tstart);
         pc.printf("pre-flight check\r\n");
         send2center();
-        i++;
     }
     
+    wait(2.0f);
     
     eskf.setPihat(updateValues.pi[0], updateValues.pi[1], palt);
     LoopTicker PIDtick;