Diff: main.cpp

Revision:

33:0f7a35d55316

Parent:

32:48d5d3f77c41

Child:

34:5719e6977ec7

--- a/main.cpp	Sat Sep 22 10:41:41 2018 +0000
+++ b/main.cpp	Sun Sep 23 18:17:50 2018 +0000
@@ -937,10 +937,14 @@
     NVIC_DisableIRQ(USART1_IRQn); 
     static int16_t pwm[6];
     static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
-    if(sbus.flg_ch_update == true){
-        
+    static int16_t FailsafeCounter=0;
+    static int16_t OKCounter=0;
+    
+    if(sbus.flg_ch_update == true && sbus.failsafe_status==SBUS_SIGNAL_OK){
+    
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
-            case Manual: 
+            case Manual:
+        if(OKCounter!=0) break;
                 for(uint8_t i=0;i<6;i++){
                     pwm[i] = sbus.manualpwm[i];  
                 }
@@ -950,37 +954,62 @@
                 break;
         
             case Auto:
+        if(OKCounter!=0) break;
                 pwm[AIL_R] = autopwm[AIL_R];               //sbus.manualpwm[AIL];
                 pwm[ELE] = autopwm[ELE];
                 pwm[THR] = autopwm[THR];
                 pwm[RUD] = autopwm[RUD];
                 pwm[DROP] = autopwm[DROP];
                 pwm[AIL_L] = autopwm[AIL_L];
-
+ 
                 NVIC_EnableIRQ(USART1_IRQn);
                 break;
                 
             default:
+        if(OKCounter!=0) break;
                 for(uint8_t i=0;i<6;i++){
                     pwm[i] = sbus.manualpwm[i];
                 } //pc.printf("update_manual\r\n");
                 NVIC_EnableIRQ(USART1_IRQn);
                 break;
-       
         }
         
         for(uint8_t i=0;i<6;i++){
-        if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
-        temppwm[i]=pwm[i];
+            if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
+            temppwm[i]=pwm[i];
+        }
+        
+    }
+    //else(sbus.flg_ch_update == false) pc.printf("0\r\n");
+   /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){
+        pc.printf("OK\r\n");
         }
-    }else{
-        pc.printf("0\r\n");
-    }
+    */
+    //pc.printf("%d\r\n",sbus.failsafe_status);
+                    
+    if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
+    else FailsafeCounter=0;
+        
+        if(FailsafeCounter>10){
+            for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i];
+            
+        if(sbus.failsafe_status==SBUS_SIGNAL_OK) OKCounter++;
+        if(OKCounter>10) {
+                OKCounter=0;
+                }
+                
+            //pc.printf("%d\r\n",sbus.failsafe_status);
+            //if(sbus.failsafe_status!=SBUS_SIGNAL_FAILSAFE)break;
+            
+        }
+    //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;}
+    
+    
     sbus.flg_ch_update = false;   
     Output_PWM(pwm);  
 }
-
-
+ 
+ 
 //pwmをサーボに出力。
 void Output_PWM(int16_t pwm[5])
 {
@@ -1217,7 +1246,7 @@
 
 //離陸-投下-着陸一連
 void Take_off_and_landing(float targetAngle[3]){
-    /*
+    
     if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
     
     switch(bombing_mode){
@@ -1233,9 +1262,10 @@
             }
             
             UpdateTargetAngle_Takeoff(targetAngle);
-            
+            NVIC_DisableIRQ(EXTI9_5_IRQn);
             if(g_distance>150) TakeoffCount++;
             else TakeoffCount = 0;
+            NVIC_EnableIRQ(EXTI9_5_IRQn);
             if(TakeoffCount>5){
                 autopwm[THR] = 1180+320*2*0.5;
                 pc.printf("Now go to Approach mode!!");
@@ -1245,7 +1275,7 @@
            
         //case Chicken:    
             //break;
-            
+        /*    
         case Transition:
             static int ApproachCount = 0;
             targetAngle[YAW]=180.0;
@@ -1254,19 +1284,19 @@
             if(Judge==0) ApproachCount++;
             if(ApproachCount>5) bombing_mode = Approach;
             break;
-            
+            */
         case Approach:
-        */
+        
             autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
             UpdateTargetAngle_Approach(targetAngle);
-            /*
+            
             break;
             
         default:
             bombing_mode = Takeoff;
             break;    
     }
-    */
+    
 }
 
 //離陸モード
@@ -1421,6 +1451,7 @@
 void checkHeight(float targetAngle[3]){
         
         static int targetHeight = 200; 
+       
         NVIC_DisableIRQ(EXTI9_5_IRQn);
         if(g_distance < targetHeight + ALLOWHEIGHT){
             UpdateTargetAngle_NoseUP(targetAngle);
@@ -1606,10 +1637,10 @@
     //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
     //pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]);
     //pc.printf("%d",g_distance);
-    NVIC_DisableIRQ(EXTI9_5_IRQn); 
-    pc.printf("g_distance = %d",g_distance);
-    NVIC_EnableIRQ(EXTI9_5_IRQn);
+    //NVIC_DisableIRQ(EXTI9_5_IRQn); 
+    //pc.printf("g_distance = %d",g_distance);
+    //NVIC_EnableIRQ(EXTI9_5_IRQn);
     //pc.printf("Mode: %c: ",g_buf[0]);
     //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
-    pc.printf("\r\n");
+    //pc.printf("\r\n");
 }
\ No newline at end of file