Diff: preflight.cpp

Revision:

144:b3a713b4f1c4

Parent:

143:53808e4e684c

--- a/preflight.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/preflight.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -42,20 +42,25 @@
             }
             // sbusデータの読み込み
             for (int i =0 ; i < 16;i ++){
-                rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
-            }
-            if (rc[4]>-0.3f && rc[6] < -0.3f){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
+                rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input
             }
-            if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
+            if (rc[7] > 0.3f){
                 preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
+                motor1.pulsewidth_us(motorPwmMin);
+                motor2.pulsewidth_us(motorPwmMin);
+                motor3.pulsewidth_us(motorPwmMin);
+                motor4.pulsewidth_us(motorPwmMin);
+                motor5.pulsewidth_us(motorPwmMin);
+                motor6.pulsewidth_us(motorPwmMin);
+                twelite.serial.printf("PreFlight Check : switch arming position\r\n");
+                pc.serial.printf("PreFlight Check : switch arming position\r\n");
             }
-            preflightCheck = true;
+            
+            
             if(preflightCheck == true){
                 break;
             }
+            pc.serial.printf("PreFlight Check : failed\r\n");
         }
     }
     twelite.serial.printf("PreFlight Check Completed\r\n");
@@ -68,12 +73,12 @@
     eskf.setPhatPosition(2.0f,2.0f);
     eskf.setPhatVelocity(1.01f,1.01f);
     eskf.setPhatAngleError(0.1f);
-    eskf.setPhatAccBias(0.5f);
+    eskf.setPhatAccBias(0.0001f);
     eskf.setPhatGyroBias(0.0001f);
     eskf.setPhatGravity(0.0000001f);
     
     eskf.setQVelocity(0.1f,0.2f);
     eskf.setQAngleError(0.0000025f);
-    eskf.setQAccBias(0.000001f);
+    eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.0000001f);
     }
\ No newline at end of file