loopとgetSFを変更

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_29 by 航空研究会

Files at this revision

API Documentation at this revision

Comitter:
taknokolat
Date:
Thu Sep 20 15:18:19 2018 +0000
Parent:
24:2cc7a3a10e72
Commit message:
a

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 2cc7a3a10e72 -r 37bee299a276 main.cpp
--- a/main.cpp	Thu Sep 20 06:58:59 2018 +0000
+++ b/main.cpp	Thu Sep 20 15:18:19 2018 +0000
@@ -371,18 +371,28 @@
 
 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);
-    //Rotate(targetAngle, 30.0);
     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);
+    
     wait_ms(23);
-    pc.printf("%c",g_landingcommand);
+    
+    NVIC_SetPriority(TIM5_IRQn,2);
+    NVIC_SetPriority(USART2_IRQn,4);
+    
+    
+   // pc.printf("6\r\n");
+    //NVIC_DisableIRQ(USART2_IRQn);
+    //pc.printf("%c",g_landingcommand);
+    //NVIC_EnableIRQ(USART2_IRQn);
 #if DEBUG_PRINT_INLOOP
     //Sbusprintf();
     DebugPrint();
@@ -1025,32 +1035,46 @@
 
 
 void getSF_Serial(){
-    
-        static char SFbuf[16];
+    //NVIC_DisableIRQ(USART1_IRQn);
+    NVIC_DisableIRQ(EXTI0_IRQn);
+    //NVIC_DisableIRQ(TIM5_IRQn);
+
+
+        static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
+
         static int bufcounter=0;
         
         if(pc.readable()) {    // 受信確認
+            
             SFbuf[bufcounter] = pc.getc();    // 1文字取り出し
-        }
-        
+          if(SFbuf[0]!='S') return;
+                }
+                
         
-        pc.printf("%c",SFbuf[bufcounter]);
-        
-        if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
+            //pc.printf("%c",SFbuf[bufcounter]);
             
-        if(bufcounter==5 && SFbuf[4]=='F'){
-            g_landingcommand = SFbuf[1];
-            wait_ms(20);
-            if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
-            bufcounter = 0;
-            memset(SFbuf, 0, strlen(SFbuf));
-            //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
-        }
+            if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
+                
+            if(bufcounter==5 && SFbuf[4]=='F'){
+                
+                g_landingcommand = SFbuf[1];
+                //wait_ms(20);
+                if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
+                bufcounter = 0;
+                memset(SFbuf, 0, strlen(SFbuf));
+                //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
+            }
+            
             else if(bufcounter>=5 ){
-                pc.printf("Communication Falsed.\r\n");
+                //pc.printf("Communication Falsed.\r\n");
+                memset(SFbuf, 0, strlen(SFbuf));
                 bufcounter = 0;
             }
-            
+        
+                    
+            //NVIC_EnableIRQ(TIM5_IRQn);
+            NVIC_EnableIRQ(EXTI0_IRQn);
+            //NVIC_EnableIRQ(USART1_IRQn); 
     }
     
 float ConvertByteintoFloat(char high, char low){