Diff: main.cpp

Revision:

52:936c90dcd872

Parent:

51:0f2f0c398fdb

--- a/main.cpp	Sat Sep 29 13:30:44 2018 +0000
+++ b/main.cpp	Sun Sep 30 01:04:14 2018 +0000
@@ -1218,6 +1218,85 @@
     int t_diff = 0;
     static int groundcount = 0;
     
+    
+    targetAngle[ROLL] = g_glideloopROLL;
+    
+    autopwm[RUD]=g_glideloopRUD;
+   // autopwm[THR]=oldTHR;
+
+    
+    
+    //時間計測開始設定
+    if(!flg_tstart && CheckSW_Up(Ch7)){
+        led4 = 0;
+        t_start = t.read();
+        flg_tstart = true;
+        pc.printf("timer start\r\n");
+    }else if(!CheckSW_Up(Ch7)){
+        t_start = 0;
+        flg_tstart = false;
+    }
+
+
+    //フラグが偽であれば計測は行わない    
+    if(flg_tstart){
+        t_diff = t.read() - t_start;
+        //一定高度or15秒でled点灯
+        NVIC_DisableIRQ(EXTI9_5_IRQn);
+        if((groundcount>5 && g_distance>0) || t_diff > 20){
+            led4 = 1;
+            //pc.printf("Call [Stop!] calling!\r\n");
+        }
+        
+        if(g_distance<280 && g_distance > 0) {
+            groundcount++;
+        }
+        NVIC_EnableIRQ(EXTI9_5_IRQn);
+    }else{
+        t_diff = 0;
+        groundcount = 0;
+        led4 = 0;
+    }
+    
+        NVIC_DisableIRQ(EXTI9_5_IRQn);
+        if(t_diff > 22){
+            autopwm[THR] = SetTHRinRatio(0.5);
+            targetAngle[PITCH] = g_autoonPITCH;
+            }
+        else if(g_distance<180 && g_distance>0 ){
+            NVIC_DisableIRQ(EXTI9_5_IRQn);
+            THRcount++;
+            if(THRcount>5) flg_ground = true;
+        }
+        else THRcount = 0;
+        NVIC_EnableIRQ(EXTI9_5_IRQn);
+        
+        if(flg_ground == true) {
+            autopwm[THR] = SetTHRinRatio(0.6);
+            targetAngle[PITCH] = g_autoonPITCH;
+            }
+        else {
+            autopwm[THR] = minpwm[THR];
+            targetAngle[PITCH] = g_glideloopPITCH;
+            }
+        
+        NVIC_DisableIRQ(USART1_IRQn);
+        if(!CheckSW_Up(Ch7)){
+            flg_ground = false;
+            }
+        NVIC_EnableIRQ(USART1_IRQn);
+}
+
+//自動滑空
+/*
+void UpdateTargetAngle_Glide(float targetAngle[3]){
+    static int THRcount = 0;
+    static int t_start = 0;
+    static bool flg_tstart = false;
+    static bool flg_ground = false;
+    int t_diff = 0;
+    static int groundcount = 0;
+    
     targetAngle[ROLL] = g_glideloopROLL;
     
     autopwm[RUD]=g_glideloopRUD;
@@ -1283,7 +1362,7 @@
             flg_ground = false;
             }
         NVIC_EnableIRQ(USART1_IRQn);
-}
+}*/
 
 //離陸-投下-着陸一連
 void Take_off_and_landing(float targetAngle[3]){