Diff: main.cpp

Revision:

5:c53ca479e96c

Parent:

4:ba610b05751d

Child:

6:13bb74a57a33

--- a/main.cpp	Sun Aug 21 01:39:04 2016 +0000
+++ b/main.cpp	Sun Aug 21 02:07:33 2016 +0000
@@ -483,14 +483,14 @@
 void StabiGyro_Climb()
 {
     if(jj<=25){     //右旋回導入
-        Auto_RUD = RUD_N - 30 - int((gy+130)*0.75);
-        Auto_ELE = ELE_N + 30 - int((gx-30)*0.8);
+        Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy
+        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
         Auto_THR = THR_N + 50;     
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((25<jj) && (jj<=224)){     //右旋回途中
-        Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
-        Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
+        Auto_RUD = RUD_N + int((-17-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN);
         Auto_THR = THR_N;
         jj++;
     /*}else if((224<jj) && (jj<=234)){    //上昇遷移
@@ -500,14 +500,14 @@
         //pc.printf("first%d\r\n",jj);
         jj++;*/
     }else if((224<jj) && (jj<=314)){    //上昇中
-        Auto_RUD = RUD_N - 10 - int((gy+25)*0.75);
-        Auto_ELE = ELE_N - 35 - int((gx-30)*0.8);
-        Auto_THR = THR_N + 220;
+        Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + int( (LAND_ANGLE + 8 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_THR = THR_N + 200;
         jj++;
     }else{                              //水平旋回
-        Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
-        Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
-        Auto_THR = THR_N;
+        Auto_RUD = RUD_N + int((-15-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy
+        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
+        Auto_THR = THR_N; 
     }
         
     if(Auto_RUD > RUD_MAX){
@@ -536,10 +536,9 @@
         //pc.printf("first%d\r\n",jj);
     //    jj++;
     //}else{
-        Auto_RUD = RUD_N - 10 - int((gy+10)*0.75);
-        Auto_ELE = ELE_N - 10 - int((gx-15)*0.8);
+        Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
         Auto_THR = 1000;    //スロットルoff
-    //}
     
     if(Auto_RUD > RUD_MAX){
         Auto_RUD = RUD_MAX;