Diff: main.cpp

Revision:

50:570782f00a76

Parent:

49:8729deed8421

Child:

51:0f2f0c398fdb

--- a/main.cpp	Fri Sep 28 05:00:55 2018 +0000
+++ b/main.cpp	Sat Sep 29 00:40:47 2018 +0000
@@ -1200,8 +1200,8 @@
 
     if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0)    {RotateCounter++; led4 = 1; pc.printf("Rotate 90\r\n");}
     if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0)  {RotateCounter++; led4 = 0; pc.printf("Rotate 180\r\n");}
-    if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-10.0)   {RotateCounter++; led4 = 1; pc.printf("Rotate 270\r\n");}
-    if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0)     {RotateCounter++; led4 = 0; pc.printf("Change Rotate direction\r\n");}
+    if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-20.0)   {RotateCounter++; led4 = 1; pc.printf("Rotate 270\r\n");}
+    if(RotateCounter == 3 && newYaw_Moebius >-10.0 && newYaw_Moebius < 90.0)     {RotateCounter++; led4 = 0; pc.printf("Change Rotate direction\r\n");}
 
 
     if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle);    
@@ -1241,12 +1241,12 @@
         t_diff = t.read() - t_start;
         //一定高度or15秒でled点灯
         NVIC_DisableIRQ(EXTI9_5_IRQn);
-        if((groundcount>5 && g_distance>0) || t_diff > 15){
+        if((groundcount>5 && g_distance>0) || t_diff > 20){
             led4 = 1;
             //pc.printf("Call [Stop!] calling!\r\n");
         }
         
-        if(g_distance<250 && g_distance > 0) {
+        if(g_distance<280 && g_distance > 0) {
             groundcount++;
         }
         NVIC_EnableIRQ(EXTI9_5_IRQn);
@@ -1257,9 +1257,11 @@
     }
     
         NVIC_DisableIRQ(EXTI9_5_IRQn);
-        //if(t_diff > 20) autopwm[THR] = SetTHRinRatio(0.5);
-        
-        if(g_distance<150 && g_distance>0 ){
+        if(t_diff > 22){
+         autopwm[THR] = SetTHRinRatio(0.6);
+         targetAngle[PITCH] = g_autoonPITCH;
+        } 
+        else if(g_distance<180 && g_distance>0 ){
             NVIC_DisableIRQ(EXTI9_5_IRQn);
             THRcount++;
             if(THRcount>5) flg_ground = true;
@@ -1269,7 +1271,7 @@
         
         if(flg_ground == true) {
             autopwm[THR] = SetTHRinRatio(0.6);
-            targetAngle[PITCH] = g_gostraightPITCH;
+            targetAngle[PITCH] = g_autoonPITCH;
             }
         else {
             autopwm[THR] = minpwm[THR];
@@ -1302,7 +1304,7 @@
             
             UpdateTargetAngle_Takeoff(targetAngle);
             NVIC_DisableIRQ(EXTI9_5_IRQn);
-            if(g_distance>100) TakeoffCount++;
+            if(g_distance>150) TakeoffCount++;
             else TakeoffCount = 0;
             NVIC_EnableIRQ(EXTI9_5_IRQn);
             if(TakeoffCount>5){
@@ -1331,7 +1333,8 @@
         case Approach:
         
             //autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
-            autopwm[THR] = 1180+320*2*0.3;
+            autopwm[THR] = minpwm[THR];//1180+320*2*0.3;
+            
             UpdateTargetAngle_Approach(targetAngle);
             
             break;
@@ -1446,9 +1449,9 @@
         switch(g_landingcommand){
             case 'R':   //右旋回セヨ
             NVIC_EnableIRQ(USART2_IRQn);
-            if(zeroTHR==false){
+            //if(zeroTHR==false){
                 UpdateTargetAngle_Rightloop_zero(targetAngle);
-                }
+                /*}
                 else{
                 targetAngle[ROLL] = g_rightloopROLL_approach;
         targetAngle[PITCH] = g_rightloopPITCH_approach ;
@@ -1459,15 +1462,15 @@
             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;
             }
-        }
+        }*/
         
                 break;
                 
                 case 'L':   //左旋回セヨ
                  NVIC_EnableIRQ(USART2_IRQn);
-                if(zeroTHR==false){
+                //if(zeroTHR==false){
                     UpdateTargetAngle_Leftloop_zero(targetAngle);
-                }
+               /* }
                     else{
                         targetAngle[ROLL] = g_leftloopROLL_approach;
                         targetAngle[PITCH] = g_leftloopPITCH_approach;
@@ -1476,28 +1479,28 @@
                             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;
                     
                 case 'G':   //直進セヨ
                 NVIC_EnableIRQ(USART2_IRQn);
-                if(zeroTHR==false){
+                //if(zeroTHR==false){
                     UpdateTargetAngle_GoStraight_zero(targetAngle);
-                    }
+                   /* }
                     else{
                     targetAngle[ROLL] = g_gostraightROLL;
                     targetAngle[PITCH] = g_gostraightPITCH;
-    }
+    }*/
                     
                     break;
                 
                 case 'Y':   //指定ノヨー方向ニ移動セヨ
                     NVIC_EnableIRQ(USART2_IRQn);
                     Rotate(targetAngle, g_SerialTargetYAW);
-                    if(zeroTHR==false){
+                    //if(zeroTHR==false){
                         autopwm[THR]=minpwm[THR];
-                        }
+                     //   }
                     break;
                     
                 case 'U':   //機首ヲ上ゲヨ