Diff: main.cpp

Revision:

31:dba3216c2755

Parent:

30:624cb32e13a3

Child:

32:48d5d3f77c41

--- a/main.cpp	Sat Sep 22 06:59:37 2018 +0000
+++ b/main.cpp	Sat Sep 22 09:46:42 2018 +0000
@@ -135,7 +135,7 @@
 
 //InterruptIn switch2(PC_14);
 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
-//HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8 
+HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8 
 
 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
@@ -181,7 +181,7 @@
 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
 
 unsigned int g_distance;
-//Ticker USsensor;
+Ticker USsensor;
 static char g_buf[16];
 char g_landingcommand='Z';
 float g_SerialTargetYAW;
@@ -332,7 +332,7 @@
     Init_sensors();
     //switch2.rise(ResetTrim);
    
-    //USsensor.attach(&UpdateDist, 0.05);
+    USsensor.attach(&UpdateDist, 0.05);
     
     NVIC_SetPriority(USART1_IRQn,0);
     NVIC_SetPriority(EXTI0_IRQn,1);
@@ -396,7 +396,7 @@
     //NVIC_EnableIRQ(USART2_IRQn);
 #if DEBUG_PRINT_INLOOP
     //Sbusprintf();
-    //DebugPrint();
+    DebugPrint();
 #endif
 }
 
@@ -1109,10 +1109,10 @@
 
 
 //超音波割り込み
-/*void UpdateDist(){
+void UpdateDist(){
     g_distance = usensor.get_dist_cm();
     usensor.start();
-}*/
+}
 
 //8の字旋回
 void UpdateTargetAngle_Moebius(float targetAngle[3]){
@@ -1149,6 +1149,7 @@
     static int THRcount = 0;
     static int t_start = 0;
     static bool flg_tstart = false;
+    static bool flg_ground = false;
     int t_diff = 0;
     static int groundcount = 0;
     
@@ -1182,8 +1183,8 @@
         }
         
         if(g_distance<180 && g_distance > 0) {
-                groundcount++;
-                }
+            groundcount++;
+        }
         NVIC_EnableIRQ(EXTI9_5_IRQn);
     }else{
         t_diff = 0;
@@ -1191,23 +1192,27 @@
         led2 = 0;
     }
     
-    if(t_diff > 17){
-        autopwm[THR] = SetTHRinRatio(0.5);
-    }else{
         NVIC_DisableIRQ(EXTI9_5_IRQn);
-        if(g_distance<150 && g_distance>0 ){
-        NVIC_EnableIRQ(EXTI9_5_IRQn);    
+        if(t_diff > 17) autopwm[THR] = SetTHRinRatio(0.5);
+        
+        else if(g_distance<150 && g_distance>0 ){
+            NVIC_DisableIRQ(EXTI9_5_IRQn);
             THRcount++;
-            if(THRcount>5){
-                autopwm[THR] = SetTHRinRatio(0.6);
-                //pc.printf("throttle ON\r\n");
+            if(THRcount>5) flg_ground = true;
+        }
+        else THRcount = 0;
+        NVIC_EnableIRQ(EXTI9_5_IRQn);
+        
+        if(flg_ground == true) autopwm[THR] = SetTHRinRatio(0.6);
+        else autopwm[THR] = minpwm[THR];
+        
+        NVIC_DisableIRQ(USART1_IRQn);
+        if(!CheckSW_Up(Ch7)){
+            flg_ground = false;
             }
-        }else{
-            autopwm[THR] = 1180;
-            THRcount = 0;
-        }
-    }
+        NVIC_EnableIRQ(USART1_IRQn);
 }
+
 //離陸-投下-着陸一連
 void Take_off_and_landing(float targetAngle[3]){
     /*
@@ -1599,8 +1604,10 @@
     //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);
-
+    NVIC_DisableIRQ(EXTI9_5_IRQn); 
+    pc.printf("g_distance = %d",g_distance);
+    NVIC_EnableIRQ(EXTI9_5_IRQn);
     //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