Diff: main.cpp

Revision:

19:0955311b0db6

Parent:

18:cce82f3374fc

Child:

20:9393b0cfa44d

--- a/main.cpp	Mon Sep 17 12:01:13 2018 +0000
+++ b/main.cpp	Mon Sep 17 13:11:53 2018 +0000
@@ -62,6 +62,7 @@
 #define AIL_L_correctionleftloop -0
 #define AIL_L_correctionleftloopshort 0
 
+
 #define RIGHTLOOP_RUD 1250 
 #define RIGHTLOOPSHORT_RUD 1250 
 #define LEFTLOOP_RUD 1500 
@@ -168,6 +169,8 @@
 
 int16_t g_AIL_L_Ratio_rightloop = 0.5;
 
+int zeroTHR=1;//着陸時のスロットルが０かの判断用
+
 
 static float nowAngle[3] = {0,0,0};
 const float trimAngle[3] = {0.0, 0.0, 0.0};
@@ -244,10 +247,13 @@
 
 //自動操縦
 void UpdateTargetAngle_GoStraight(float targetAngle[3]);
+void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]);       //着陸時にスロットルが０の時の直進
 void UpdateTargetAngle_Rightloop(float targetAngle[3]);
 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]);      //着陸時にスロットルが０の時の右旋回
 void UpdateTargetAngle_Leftloop(float targetAngle[3]);
 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]);     //着陸時にスロットルが０の時の左旋回
 void UpdateTargetAngle_Moebius(float targetAngle[3]);
 void UpdateTargetAngle_Glide(float targetAngle[3]);
 void UpdateTargetAngle_Takeoff(float targetAngle[3]);
@@ -1275,6 +1281,10 @@
        
         switch(g_landingcommand){
             case 'R':   //右旋回セヨ
+            if(zeroTHR==0){
+                UpdateTargetAngle_Rightloop_zero(targetAngle);
+                }
+                else{
                 targetAngle[ROLL] = g_rightloopROLL;
         targetAngle[PITCH] = g_rightloopPITCH ;
         autopwm[RUD]=g_rightloopRUD;              //RUD固定
@@ -1285,8 +1295,12 @@
         autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
         
                 break;
-                
+                }
                 case 'L':   //左旋回セヨ
+                if(zeroTHR==0){
+                    UpdateTargetAngle_Leftloop_zero(targetAngle);
+                    }
+                    else{
                     targetAngle[ROLL] = g_leftloopROLL;
             targetAngle[PITCH] = g_leftloopPITCH;
             autopwm[RUD]=g_leftloopRUD;
@@ -1294,13 +1308,17 @@
                 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':   //直進セヨ
+                if(zeroTHR==0){
+                    UpdateTargetAngle_GoStraight_zero(targetAngle);
+                    }
+                    else{
                     targetAngle[ROLL] = g_gostraightROLL;
                     targetAngle[PITCH] = g_gostraightPITCH;
-    
+    }
     
                     break;
                 
@@ -1320,6 +1338,7 @@
                     targetAngle[ROLL] = 0.0;
                     targetAngle[PITCH] = -3.0;
                     autopwm[THR] = minpwm[THR];
+                    zeroTHR=0;
                     break;
             
             }
@@ -1360,6 +1379,7 @@
 //直進
 void UpdateTargetAngle_GoStraight(float targetAngle[3]){
 
+    autopwm[RUD] = trimpwm[RUD];
     targetAngle[ROLL] = g_gostraightROLL;
     targetAngle[PITCH] = g_gostraightPITCH;
     autopwm[THR] = SetTHRinRatio(g_loopTHR);
@@ -1367,6 +1387,16 @@
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
 
+//直進(着陸時スロットル０のとき）
+void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]){
+
+    autopwm[RUD] =  trimpwm[RUD];
+    targetAngle[ROLL] = g_gostraightROLL;
+    targetAngle[PITCH] = g_gostraightPITCH;
+    autopwm[THR] = minpwm[THR];
+    
+    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
 
 //右旋回
 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
@@ -1406,7 +1436,7 @@
     targetAngle[ROLL] = g_rightloopROLLshort;
     targetAngle[PITCH] = g_rightloopPITCHshort;
     autopwm[RUD]=g_rightloopshortRUD;
-    autopwm[THR] = oldTHR;
+    autopwm[THR] = SetTHRinRatio(0.5);
     if(autopwm[AIL_R]>trimpwm[AIL_R]){
         autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
         }
@@ -1421,7 +1451,7 @@
     targetAngle[ROLL] = g_leftloopROLL;
     targetAngle[PITCH] = g_leftloopPITCH;
     autopwm[RUD]=g_leftloopRUD;
-    autopwm[THR] = oldTHR;
+    autopwm[THR] = SetTHRinRatio(0.5);
     if(autopwm[AIL_R]>trimpwm[AIL_R]){
         autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
         }
@@ -1453,7 +1483,7 @@
     targetAngle[ROLL] = g_leftloopROLLshort;
     targetAngle[PITCH] = g_leftloopPITCHshort;
     autopwm[RUD]=g_leftloopRUD;
-    autopwm[THR] = oldTHR;
+    autopwm[THR] = SetTHRinRatio(0.5);
     if(autopwm[AIL_R]>trimpwm[AIL_R]){
         autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
         }