Diff: main.cpp

Revision:

33:bcc14ca08e84

Parent:

32:9d0646372abe

diff -r 9d0646372abe -r bcc14ca08e84 main.cpp
--- a/main.cpp	Sun Sep 23 03:20:58 2018 +0000
+++ b/main.cpp	Sun Sep 23 07:50:41 2018 +0000
@@ -274,6 +274,7 @@
 void UpdateTargetAngle_NoseUP(float targetAngle[3]);
 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]);
 
+
 //デバッグ用
 void Sbusprintf();
 void DebugPrint();
@@ -379,7 +380,6 @@
     UpdateTargetAngle(targetAngle);
     CalculateControlValue(targetAngle, controlValue);
     UpdateAutoPWM(controlValue);
-   
     
     //NVIC_SetPriority(TIM5_IRQn,4);
     //NVIC_SetPriority(USART2_IRQn,2);
@@ -931,55 +931,6 @@
 
 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
-/*
-void Update_PWM()
-{   
-    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){
-        
-        switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
-            case Manual: 
-                for(uint8_t i=0;i<6;i++){
-                    pwm[i] = sbus.manualpwm[i];  
-                }
-                oldTHR = sbus.manualpwm[THR];
-                //pc.printf("update_manual\r\n");
-                NVIC_EnableIRQ(USART1_IRQn);
-                break;
-        
-            case Auto:
-                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:
-                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];
-        }
-    }else{
-        pc.printf("0\r\n");
-    }
-    sbus.flg_ch_update = false;   
-    Output_PWM(pwm);  
-}
-*/
 
 /*---SBUS割り込み処理---*/
 
@@ -990,7 +941,7 @@
     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]};
-    static int16_t FailsafeCounter=0;
+    //static int16_t FailsafeCounter=0;
     
     if(sbus.flg_ch_update == true){
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
@@ -1032,9 +983,8 @@
    /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){
         pc.printf("OK\r\n");
         }
-    */
-        
-    if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
+    */  
+    /*if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
     else FailsafeCounter=0;
         
         if(FailsafeCounter>10){
@@ -1057,7 +1007,7 @@
                 //if(sbus.failsafe_status!=SBUS_SIGNAL_FAILSAFE)break;
             } 
         }
-    
+   */
     sbus.flg_ch_update = false;   
     Output_PWM(pwm);  
 }
@@ -1690,6 +1640,8 @@
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
 
+
+
 void Sbusprintf(){
     
     for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]);