pc,attachの変更

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_36 by 航空研究会

Revision:
28:aa44903a01e1
Parent:
27:61876b34ded4
Child:
29:887a38540508
--- a/main.cpp	Fri Sep 21 13:11:19 2018 +0000
+++ b/main.cpp	Fri Sep 21 20:03:32 2018 +0000
@@ -376,21 +376,18 @@
 void loop(){
     static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
     SensingMPU();
-    NVIC_DisableIRQ(USART2_IRQn);
     UpdateTargetAngle(targetAngle);
     CalculateControlValue(targetAngle, controlValue);
-    NVIC_DisableIRQ(USART1_IRQn);
     UpdateAutoPWM(controlValue);
-    NVIC_EnableIRQ(USART1_IRQn);
-    NVIC_EnableIRQ(USART2_IRQn);
+   
     
-    NVIC_SetPriority(TIM5_IRQn,4);
-    NVIC_SetPriority(USART2_IRQn,2);
+    //NVIC_SetPriority(TIM5_IRQn,4);
+    //NVIC_SetPriority(USART2_IRQn,2);
     
     wait_ms(23);
     
-    NVIC_SetPriority(TIM5_IRQn,2);
-    NVIC_SetPriority(USART2_IRQn,4);
+    //NVIC_SetPriority(TIM5_IRQn,2);
+    //NVIC_SetPriority(USART2_IRQn,4);
     
     
    // pc.printf("6\r\n");
@@ -399,7 +396,7 @@
     //NVIC_EnableIRQ(USART2_IRQn);
 #if DEBUG_PRINT_INLOOP
     //Sbusprintf();
-    DebugPrint();
+    //DebugPrint();
 #endif
 }
 
@@ -419,7 +416,7 @@
     servo4.pulsewidth_us(trimpwm[RUD]);
     
     servo5.period_ms(14);
-    servo5.pulsewidth_us(1392);
+    servo5.pulsewidth_us(trimpwm[DROP]);
 
     servo6.period_ms(14);
     servo6.pulsewidth_us(trimpwm[AIL_L]);
@@ -459,6 +456,8 @@
 }
 
 void UpdateTargetAngle(float targetAngle[3]){
+    
+    
     static int16_t count_op = 0;
 #if DEBUG_SEMIAUTO    
     switch(operation_mode){
@@ -595,6 +594,7 @@
             led1 = 0;
         }
 
+   
 }
 
 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
@@ -869,6 +869,7 @@
 }                  
 
 void CalculateControlValue(float targetAngle[3], float controlValue[3]){
+    
     static int t_last;
     int t_now;
     float dt;
@@ -881,9 +882,11 @@
     //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
     controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);       //エルロンでロール制御   
     controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
+    
 }
 
-void UpdateAutoPWM(float controlValue[3]){    
+void UpdateAutoPWM(float controlValue[3]){ 
+    NVIC_DisableIRQ(USART1_IRQn);   
     int16_t addpwm[2]; //-500~500
     addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH];    //センサ:機首下げ正    レバー:機首上げ正
     addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ:右回転正(8月13日時点;左回転が正!)     レバー:右回転正
@@ -895,6 +898,7 @@
     autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
     autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
     
+    NVIC_EnableIRQ(USART1_IRQn);
 }
 
 inline float CalcRatio(float value, float trim, float limit){
@@ -902,13 +906,13 @@
 }
 
 bool CheckSW_Up(Channel ch){
-    NVIC_DisableIRQ(USART1_IRQn);
+    
     if(SWITCH_CHECK < sbus.manualpwm[ch]){
         return true;
     }else{
         return false;
     }
-    NVIC_EnableIRQ(USART1_IRQn);
+    
 }
 
 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){
@@ -928,11 +932,12 @@
 //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){
-        NVIC_DisableIRQ(USART1_IRQn);
+        
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
             case Manual: 
                 for(uint8_t i=0;i<6;i++){
@@ -942,6 +947,7 @@
                 pc.printf("%d ,\r\n",pwm[5]);//L*/
                 oldTHR = sbus.manualpwm[THR];
                 //pc.printf("update_manual\r\n");
+                NVIC_EnableIRQ(USART1_IRQn);
                 break;
         
             case Auto:
@@ -954,16 +960,18 @@
                 //pc.printf("%d ,",pwm[AIL_R]);//R
                 //pc.printf("%d ,\r\n",pwm[AIL_L]);//L
                 //pc.printf("update_auto\r\n");
-                
+                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;
-            NVIC_EnableIRQ(USART1_IRQn);
+       
         }
+        
         for(uint8_t i=0;i<6;i++){
         if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
         temppwm[i]=pwm[i];
@@ -1048,7 +1056,7 @@
 
 void getSF_Serial(){
     //NVIC_DisableIRQ(USART1_IRQn);
-    NVIC_DisableIRQ(EXTI0_IRQn);
+    //NVIC_DisableIRQ(EXTI0_IRQn);
     //NVIC_DisableIRQ(TIM5_IRQn);
 
 
@@ -1056,10 +1064,15 @@
 
         static int bufcounter=0;
         
+       
+        
         if(pc.readable()) {    // 受信確認
             
             SFbuf[bufcounter] = pc.getc();    // 1文字取り出し
-          if(SFbuf[0]!='S') return;
+          if(SFbuf[0]!='S'){
+                 //pc.printf("x");
+                 return;
+          }  
                 }
                 
         
@@ -1074,18 +1087,20 @@
                 if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
                 bufcounter = 0;
                 memset(SFbuf, 0, strlen(SFbuf));
+                NVIC_ClearPendingIRQ(USART2_IRQn);
                 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
             }
             
-            else if(bufcounter>=5 ){
+            else if(bufcounter>=5){
                 //pc.printf("Communication Falsed.\r\n");
                 memset(SFbuf, 0, strlen(SFbuf));
                 bufcounter = 0;
+                NVIC_ClearPendingIRQ(USART2_IRQn);
             }
         
                     
             //NVIC_EnableIRQ(TIM5_IRQn);
-            NVIC_EnableIRQ(EXTI0_IRQn);
+            //NVIC_EnableIRQ(EXTI0_IRQn);
             //NVIC_EnableIRQ(USART1_IRQn); 
     }
     
@@ -1171,7 +1186,9 @@
             //pc.printf("Call [Stop!] calling!\r\n");
         }
         
-        if(g_distance<180 && g_distance > 0) groundcount++;
+        if(g_distance<180 && g_distance > 0) {
+                groundcount++;
+                }
         NVIC_EnableIRQ(EXTI9_5_IRQn);
     }else{
         t_diff = 0;
@@ -1306,7 +1323,7 @@
     }
     
     //着陸モード(PCからの指令に従う)
-    void UpdateTargetAngle_Approach(float targetAngle[3]){
+void UpdateTargetAngle_Approach(float targetAngle[3]){
        
        static bool zeroTHR=true;//着陸時のスロットル動作確認用
        
@@ -1320,8 +1337,7 @@
         }
     
     
-        NVIC_DisableIRQ(USART2_IRQn);
-       
+       NVIC_DisableIRQ(USART2_IRQn);
         switch(g_landingcommand){
             case 'R':   //右旋回セヨ
             if(zeroTHR==false){
@@ -1331,29 +1347,31 @@
                 targetAngle[ROLL] = g_rightloopROLL;
         targetAngle[PITCH] = g_rightloopPITCH ;
         autopwm[RUD]=g_rightloopRUD;              //RUD固定
-        if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
-            autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+            if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
+                autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+                }
+            else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+            autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
             }
-        else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
-        autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
         }
-    }
+        NVIC_EnableIRQ(USART2_IRQn);
                 break;
                 
                 case 'L':   //左旋回セヨ
                 if(zeroTHR==false){
                     UpdateTargetAngle_Leftloop_zero(targetAngle);
-                    }
+                }
                     else{
-                    targetAngle[ROLL] = g_leftloopROLL;
-            targetAngle[PITCH] = g_leftloopPITCH;
-            autopwm[RUD]=g_leftloopRUD;
-            if(autopwm[AIL_R]<trimpwm[AIL_R]){
-                autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
-                }
-            else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
-        }
-                    break;
+                        targetAngle[ROLL] = g_leftloopROLL;
+                        targetAngle[PITCH] = g_leftloopPITCH;
+                        autopwm[RUD]=g_leftloopRUD;
+                        if(autopwm[AIL_R]<trimpwm[AIL_R]){
+                            autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+                        }
+                        else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+                    }
+                NVIC_EnableIRQ(USART2_IRQn);
+                break;
                     
                 case 'G':   //直進セヨ
                 if(zeroTHR==false){
@@ -1363,19 +1381,22 @@
                     targetAngle[ROLL] = g_gostraightROLL;
                     targetAngle[PITCH] = g_gostraightPITCH;
     }
-    
+                    NVIC_EnableIRQ(USART2_IRQn);
                     break;
                 
                 case 'Y':   //指定ノヨー方向ニ移動セヨ
                     Rotate(targetAngle, g_SerialTargetYAW);
+                    NVIC_EnableIRQ(USART2_IRQn);
                     break;
                 
                 case 'B':   //ブザーヲ鳴ラセ
                     //buzzer = 1;
+                    NVIC_EnableIRQ(USART2_IRQn);
                     break;
                     
                 case 'D':   //物資ヲ落トセ
                     Chicken_Drop();
+                    NVIC_EnableIRQ(USART2_IRQn);
                     break;
                             
                 case 'C':   //停止セヨ
@@ -1383,10 +1404,16 @@
                     targetAngle[PITCH] = -3.0;
                     autopwm[THR] = minpwm[THR];
                     zeroTHR=false;
+                    NVIC_EnableIRQ(USART2_IRQn);
                     break;
+                    
+                default :
+                NVIC_EnableIRQ(USART2_IRQn);
+                break;
+                
             
             }
-            NVIC_EnableIRQ(USART2_IRQn);
+            
 }
         
 void checkHeight(float targetAngle[3]){
@@ -1449,13 +1476,13 @@
     autopwm[RUD]=g_rightloopRUD;              //RUD固定
     autopwm[THR] = SetTHRinRatio(0.5);                  //手動スロットル記憶
     
-    
+    /*
     if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){
         t2.start();
         pc.printf("Timer start.");
     }
     if(0.0<t2.read()<5.0){
-            pc.printf("tagetAngle is changed.");
+            //pc.printf("tagetAngle is changed.");
             targetAngle[ROLL] = rightloopROLL2;
             }
         else {
@@ -1464,7 +1491,7 @@
             pc.printf("Timer stopped.");
             targetAngle[ROLL] = g_rightloopROLL;
         }    
-
+*/
     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
         autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
         }
@@ -1573,12 +1600,12 @@
     //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
     //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]);
     //pc.printf("\r\n");
-    for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
+    //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
     //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);
 
     //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