Diff: main.cpp

Revision:

4:fff1165ca50c

Parent:

3:206b17251f5b

Child:

5:9efd35c9bb2e

--- a/main.cpp	Thu Sep 13 11:34:19 2018 +0000
+++ b/main.cpp	Thu Sep 13 12:06:21 2018 +0000
@@ -1215,6 +1215,22 @@
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
 
+//右旋回（着陸時スロットル０の時）
+void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回
+
+    targetAngle[ROLL] = g_rightloopROLL;
+    targetAngle[PITCH] = g_rightloopPITCH ;
+    autopwm[RUD]=rightloopRUD;
+    autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR);
+    if(autopwm[AIL_R]>trimpwm[AIL_R]){
+        autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+        }
+    else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+
+    //checkHeight(targetAngle);
+    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回
 
     targetAngle[ROLL] = g_rightloopROLLshort;
@@ -1234,7 +1250,29 @@
     
     targetAngle[ROLL] = g_leftloopROLL;
     targetAngle[PITCH] = g_leftloopPITCH;
-    autopwm[THR] = SetTHRinRatio(g_loopTHR);
+    autopwm[RUD]=leftloopRUD;
+    autopwm[THR] = oldTHR;
+    if(autopwm[AIL_R]>trimpwm[AIL_R]){
+        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;
+    //checkHeight(targetAngle);
+
+    //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
+}
+
+//左旋回（着陸時スロットル０のとき）
+void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){
+    
+    targetAngle[ROLL] = g_leftloopROLL;
+    targetAngle[PITCH] = g_leftloopPITCH;
+    autopwm[RUD]=leftloopRUD;
+    autopwm[THR] = minpwm[THR];
+    if(autopwm[AIL_R]>trimpwm[AIL_R]){
+        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;
+    //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop;
     //checkHeight(targetAngle);
 
     //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
@@ -1244,8 +1282,13 @@
     
     targetAngle[ROLL] = g_leftloopROLLshort;
     targetAngle[PITCH] = g_leftloopPITCHshort;
-    autopwm[THR] = SetTHRinRatio(g_loopTHR);
-    
+    autopwm[RUD]=leftloopRUD;
+    autopwm[THR] = oldTHR;
+    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;
+        }
+    else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
 void ISR_Serial_Rx()