Diff: main.cpp

Revision:

7:16bf0085d914

Parent:

6:a50e6d3924f1

Child:

8:1db19b529b22

--- a/main.cpp	Tue Feb 24 09:28:29 2015 +0000
+++ b/main.cpp	Tue Apr 28 01:48:21 2015 +0000
@@ -533,7 +533,7 @@
 void PWM_Out(bool mode)
 {
     int reg[3];
-    int i;
+    int i,j,k;
     float gain;
 //    float cur_height;
     
@@ -563,9 +563,17 @@
             M5 = THR + conf.Throttl_Trim;
             break;
         case Quad_3D:
-            i = (int)throLimit.calc((float)THR);
-            if ( abs(i-conf.Reverse_Point) > 100 ) i = THR;
-            M1 = M2 = M3 = M4 = i + conf.Throttl_Trim;
+            j = THR + conf.Throttl_Trim;
+            if ( Stick[GAIN] < 0 )  {
+                k = THR - conf.Reverse_Point;
+                if ( abs(k) < THR_MIDRANGE )   { 
+                    if ( k > 0 ) j = conf.Reverse_Point + THR_MIDRANGE;
+                    else j = conf.Reverse_Point - THR_MIDRANGE;
+                }
+            }
+//           i = (int)throLimit.calc((float)THR);
+ //           if ( abs(i-conf.Reverse_Point) > 100 ) i = THR;
+            M1 = M2 = M3 = M4 = j;
             break;
         case Delta:
         case Delta_TW:
@@ -655,32 +663,28 @@
         M4 -= reg[YAW];
         break;
     }
-    int h = conf.Model_Type;
-    for ( int i=0; i<6; i++ ) {
+    j = conf.Model_Type;
+    for ( i=0; i<6; i++ ) {
         if ( M[i] > Pulse_Max )   M[i] = Pulse_Max; 
         if ( M[i] < Pulse_Min )   M[i] = Pulse_Min;
-        if ( Servo_idx[h][i] == 1 )
+        if ( Servo_idx[j][i] == 1 )
         {
             M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] );
         }
         else    {
-            if ( h == Quad_3D ) {
+            if ( j == Quad_3D ) {
                 if ( Stick[GAIN] > 0 )  {
                     if ( THR < conf.Reverse_Point ) 
                         M[i] = conf.Reverse_Point;
                 }
-                else    {
-                    if ( abs(THR - conf.Reverse_Point ) < 10 )
-                        M[i] = conf.Reverse_Point;
-                }            
             }
             else    {
- //               if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL];
+                if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL];
             }
         }
     }
     if ( mode ) {
-        h = conf.Stick_Ref[THR];
+//        h = conf.Stick_Ref[THR];
         for ( i=0;i<6;i++ ) {
 //           while ( !pwmpin[i] );
             if ( conf.PWM_Mode == 1 )