Diff: main.cpp

Revision:

34:5719e6977ec7

Parent:

33:0f7a35d55316

Child:

35:25e1afadd455

--- a/main.cpp	Sun Sep 23 18:17:50 2018 +0000
+++ b/main.cpp	Mon Sep 24 04:24:51 2018 +0000
@@ -1268,7 +1268,8 @@
             NVIC_EnableIRQ(EXTI9_5_IRQn);
             if(TakeoffCount>5){
                 autopwm[THR] = 1180+320*2*0.5;
-                pc.printf("Now go to Approach mode!!");
+                autopwm[ELE] = 1200;
+                //pc.printf("Now go to Approach mode!!");
                 bombing_mode = Approach;
             }
             break;
@@ -1302,11 +1303,33 @@
 //離陸モード
 void UpdateTargetAngle_Takeoff(float targetAngle[3]){
     //pc.printf("%d \r\n",g_distance);
+    static int tELE_start = 0;
+    static bool flg_ELEup = false;
+    int t_def = 0;
+    if(!flg_ELEup && CheckSW_Up(Ch7)){
+        tELE_start = t.read_ms();
+        flg_ELEup = true;
+        pc.printf("timer start\r\n");
+    }else if(!CheckSW_Up(Ch7)){
+        tELE_start = 0;
+        flg_ELEup = false;
+    }
+    if(flg_ELEup){
+        t_def = t.read_ms() - tELE_start;
+        
+        //1.5秒経過すればELE上げ舵へ
+        if(t_def>500) targetAngle[PITCH]=-30.0;
+        else{
+            t_def = 0;
+            targetAngle[PITCH]=g_gostraightPITCH;
+        }
+    }
     targetAngle[ROLL] = g_gostraightROLL;
-    targetAngle[PITCH] = g_loopTHR;
+    //targetAngle[PITCH] = g_loopTHR;
     autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
 }
 
+
 //ヨーを目標値にして許容角度になるまで水平旋回
 int Rotate(float targetAngle[3], float TargetYAW){
     float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);