Diff: main.cpp

Revision:

43:4413ee61bc39

Parent:

42:74d72339a8a8

--- a/main.cpp	Wed Sep 26 04:35:47 2018 +0000
+++ b/main.cpp	Wed Sep 26 11:26:28 2018 +0000
@@ -961,9 +961,10 @@
     static int16_t pwm[6];
     static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
     static int16_t FailsafeCounter=0;
+    static int16_t ResetCounter=0;
     static int16_t OKCounter=0;
     
-    if(sbus.flg_ch_update == true && sbus.failsafe_status==SBUS_SIGNAL_OK){
+    if(sbus.flg_ch_update == true){
     
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
             case Manual:
@@ -1011,20 +1012,25 @@
     //pc.printf("%d\r\n",sbus.failsafe_status);
                     
     if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
+    else ResetCounter++;
         
+    if(ResetCounter>7){
+        ResetCounter=0;
+        FailsafeCounter=0;
+    }
+
         if(FailsafeCounter>10){
+        ResetCounter=0;
             for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i];
             
-        if(sbus.failsafe_status==SBUS_SIGNAL_OK) OKCounter++;
+        if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++;
         else OKCounter=0;
         
-        if(OKCounter>5){
+        if(OKCounter>10){
         OKCounter=0;
-        FailsafeCounter=0;   
+                FailsafeCounter=0;   
         }
-            //pc.printf("%d\r\n",sbus.failsafe_status);
-            //if(sbus.failsafe_status!=SBUS_SIGNAL_FAILSAFE)break;
-            
+            pc.printf("OKCounter=%d,  FailsafeCounter=%d,  sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status);
         }
     //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;}
     
@@ -1034,6 +1040,8 @@
 }
  
  
+ 
+ 
 //pwmをサーボに出力。
 void Output_PWM(int16_t pwm[5])
 {
@@ -1299,6 +1307,7 @@
             if(TakeoffCount>5){
                 autopwm[THR] = 1180+320*2*0.5;
                 targetAngle[PITCH]=g_gostraightPITCH;
+                autopwm[RUD]=trimpwm[RUD];
                 //pc.printf("Now go to Approach mode!!");
                 bombing_mode = Approach;
             }