Revision 33:1a377cb25565, committed 2018-09-23

Comitter:

HARUKIDELTA

Date:

Sun Sep 23 08:12:44 2018 +0000

Parent:

32:9d0646372abe

Commit message:

FAILSAFE????

Changed in this revision

--- a/main.cpp	Sun Sep 23 03:20:58 2018 +0000
+++ b/main.cpp	Sun Sep 23 08:12:44 2018 +0000
@@ -69,7 +69,7 @@
 #define RIGHTLOOPSHORT_RUD 1250 
 #define LEFTLOOP_RUD 1500 
 #define LEFTLOOPSHORT_RUD 1500
-#define GLIDELOOP_RUD 1300  
+#define GLIDELOOP_RUD 1250  
 #define AIL_L_CORRECTION_RIGHTLOOP 0
 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
 #define AIL_L_CORRECTION_LEFTLOOP 0
@@ -993,7 +993,7 @@
     static int16_t FailsafeCounter=0;
     
     if(sbus.flg_ch_update == true){
-        switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
+        switch(output_status && sbus.failsafe_status==SBUS_SIGNAL_OK){    //マニュアルモード,自動モード,自動着陸もモードを切替
             case Manual: 
                 for(uint8_t i=0;i<6;i++){
                     pwm[i] = sbus.manualpwm[i];  
@@ -1033,14 +1033,14 @@
         pc.printf("OK\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];
             
-            while(sbus.failsafe_status!=SBUS_SIGNAL_OK){
                 servo1.pulsewidth_us(pwm[0]);
                 servo2.pulsewidth_us(pwm[1]);
                 servo3.pulsewidth_us(pwm[2]);
@@ -1052,11 +1052,12 @@
                 led3 = !led3;
                 led4 = !led4;
                 wait_ms(35);
-                FailsafeCounter=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);  
@@ -1236,10 +1237,13 @@
     static bool flg_ground = false;
     int t_diff = 0;
     static int groundcount = 0;
+    static int checkELE;
     
     targetAngle[ROLL] = g_glideloopROLL;
+    
+    if(checkELE==0){
     targetAngle[PITCH] = g_glideloopPITCH;
-    
+    }
     autopwm[RUD]=g_glideloopRUD;
    // autopwm[THR]=oldTHR;
 
@@ -1249,6 +1253,7 @@
     if(!flg_tstart && CheckSW_Up(Ch7)){
         t_start = t.read();
         flg_tstart = true;
+        checkELE = 0;
         pc.printf("timer start\r\n");
     }else if(!CheckSW_Up(Ch7)){
         t_start = 0;
@@ -1287,7 +1292,10 @@
         else THRcount = 0;
         NVIC_EnableIRQ(EXTI9_5_IRQn);
         
-        if(flg_ground == true) autopwm[THR] = SetTHRinRatio(0.6);
+        if(flg_ground == true) {
+            autopwm[THR] = SetTHRinRatio(0.8);
+            checkELE=1;
+            autopwm[ELE] = 1200; }
         else autopwm[THR] = minpwm[THR];
         
         NVIC_DisableIRQ(USART1_IRQn);