Revision 18:f3dcb174b563, committed 2018-09-17

Comitter:

taknokolat

Date:

Mon Sep 17 10:33:13 2018 +0000

Parent:

17:55249ea37dff

Commit message:

a

Changed in this revision

diff -r 55249ea37dff -r f3dcb174b563 main.cpp
--- a/main.cpp	Mon Sep 17 10:07:27 2018 +0000
+++ b/main.cpp	Mon Sep 17 10:33:13 2018 +0000
@@ -168,6 +168,8 @@
 
 int16_t g_AIL_L_Ratio_rightloop = 0.5;
 
+int zeroTHR=0;
+
 
 static float nowAngle[3] = {0,0,0};
 const float trimAngle[3] = {0.0, 0.0, 0.0};
@@ -244,10 +246,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]);
@@ -1272,6 +1277,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固定
@@ -1280,10 +1289,14 @@
             }
         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':   //左旋回セヨ
+                if(zeroTHR==0){
+                    UpdateTargetAngle_Leftloop_zero(targetAngle);
+                    }
+                    else{
                     targetAngle[ROLL] = g_leftloopROLL;
             targetAngle[PITCH] = g_leftloopPITCH;
             autopwm[RUD]=g_leftloopRUD;
@@ -1291,13 +1304,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':   //直進セヨ
-                    targetAngle[ROLL] = g_gostraightROLL;
+                if(zeroTHR==0){
+                    UpdateTargetAngle_GoStraight_zero(targetAngle);
+                    }
+                    else{
+                    autopwm[RUD]=1404;
                     targetAngle[PITCH] = g_gostraightPITCH;
-    
+    }
     
                     break;
                 
@@ -1314,9 +1331,10 @@
                     break;
                             
                 case 'C':   //停止セヨ
-                    targetAngle[ROLL] = 0.0;
+                    autopwm[RUD]=1404;
                     targetAngle[PITCH] = -3.0;
                     autopwm[THR] = minpwm[THR];
+                    zeroTHR=0;
                     break;
             
             }
@@ -1357,13 +1375,23 @@
 //直進
 void UpdateTargetAngle_GoStraight(float targetAngle[3]){
 
-    targetAngle[ROLL] = g_gostraightROLL;
+    autopwm[RUD]=1404;
     targetAngle[PITCH] = g_gostraightPITCH;
     autopwm[THR] = SetTHRinRatio(g_loopTHR);
     
     //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]=1404;
+    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]){ //右旋回