Revision 6:a50e6d3924f1, committed 2015-02-24

Comitter:

komaida424

Date:

Tue Feb 24 09:28:29 2015 +0000

Parent:

5:7b02775787a9

Child:

7:16bf0085d914

Commit message:

Release?revision 3.1

Changed in this revision

--- a/Limiter/Limiter.cpp	Fri Oct 17 03:56:56 2014 +0000
+++ b/Limiter/Limiter.cpp	Tue Feb 24 09:28:29 2015 +0000
@@ -3,24 +3,35 @@
 
 Limiter::Limiter(float LIMIT)
 {
-    limit = LIMIT;   
-    last= 0;
+    _limit = LIMIT;
+    _rate = 0.05;   
+    _last= 0;
     skip = false;
 }
 
+void Limiter::differential(float DIFF)
+{
+    _limit = DIFF;
+}
+
+void Limiter::rate(float RATE)
+{
+    _rate = RATE;    
+}
+
 float Limiter::calc(float now)
 {
     if ( !skip ) { 
         skip = true; 
-        return last = now;
+        return _last = now;
     }
-    float differential = ( now - last );
-    if ( differential < limit && differential > -limit ) last = now;
-    if ( differential > limit ) last += limit * 0.05f;
+    float differential = ( now - _last );
+//    if ( differential < _limit && differential > -_limit ) _last = now;
+    if ( differential > _limit ) _last += _limit * _rate;
     else    {
-        if ( differential < -limit ) last -= limit * 0.05f;
-        else last = now;
+        if ( differential < -_limit ) _last -= _limit * _rate;
+        else _last = now;
     }
-    return last;
+    return _last;
 }
 ;

--- a/Limiter/Limiter.h	Fri Oct 17 03:56:56 2014 +0000
+++ b/Limiter/Limiter.h	Tue Feb 24 09:28:29 2015 +0000
@@ -8,10 +8,13 @@
 {
 public:
     Limiter(float limit);
-    float calc(float now);
+    void differential(float);
+    void rate(float);
+    float calc(float);
 private:
-    float last;
-    float limit;
+    float _last;
+    float _limit;
+    float _rate;
     bool skip;
 };
 #endif
\ No newline at end of file

--- a/config.cpp	Fri Oct 17 03:56:56 2014 +0000
+++ b/config.cpp	Tue Feb 24 09:28:29 2015 +0000
@@ -4,6 +4,7 @@
 #include "config.h"
 #include "PulseWidthCounter.h"
 #include "SerialLcd.h"
+#include "Limiter.h"
 //#include "PID.h"
 
 //Serial pc(USBTX, USBRX); 
@@ -58,12 +59,13 @@
 extern volatile int Stick[6];
 extern volatile float Press;
 extern volatile float interval;
+extern Limiter throLimit;
 //extern bool tick_flag;
 //extern PID pid[4];
 //extern PID height;
 //extern int pid_reg[4];
 const char steering[3][6]= {"Roll ","Pitch","Yaw  "};
-const char ModelName[5][9] = { "Quad-X  ","Quad-H  ","Delta   ","Delta-TW","AirPlane" };    
+const char ModelName[6][9] = { "Quad-X  ","Quad-VP ","Quad-3D ","Delta   ","Delta-TW","AirPlane" };    
 int mode;//
 char sw,ret_mode;
 int vnum,hnum,vmax,hmax;//
@@ -96,7 +98,23 @@
                 LCD_printf(str);
                 LCD_locate(4,1);
                 LCD_printf("By AZUKITEN");
-                hmax = 0;
+                hmax = 1;
+                break;
+            case 1:         //モデルタイプの設定
+                LCD_locate(0,0);
+                LCD_printf("Model Type");
+                LCD_locate(0,1);
+                switch ( sw ) {
+                    case 'D':
+                        if ( conf.Model_Type > 0 ) conf.Model_Type -= 1;
+                        else conf.Model_Type = 4;
+                        break;
+                    case 'U':
+                        if ( conf.Model_Type < 5 ) conf.Model_Type += 1;
+                        else conf.Model_Type = 0;
+                }
+                LCD_printf( (char*)ModelName[conf.Model_Type] );
+                Set_Arrow(2);
                 break;
 
             //送信機信号のキャリブレーション
@@ -137,7 +155,7 @@
             case GYROGAIN*10:        //Set Gyro Gain
                 LCD_printf("Set Gyro Gain");
                 Set_Arrow(1);
-                hmax = 4;
+                hmax = 5;
                 break;
             case GYROGAIN*10+1:                                //Set Gyro Gain Roll
                 if ( conf.Gyro_Gain_Setting == 1 )
@@ -158,6 +176,9 @@
                     Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
                 break;
             case GYROGAIN*10+4:
+                Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
+                break;
+            case GYROGAIN*10+5:
 //                ret_mode = 'R';
                 LCD_printf("GyroGain>setting");
                 LCD_locate(0,1);
@@ -279,7 +300,6 @@
                 LCD_printf("Disp Pulse Width");
                 Set_Arrow(1);
                 hmax = 3;
-                x = conf.ESC_Low;
                 break;
             case DISPPULSE*10+1:        //Display Pulse Width
 //           DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
@@ -405,20 +425,18 @@
             case DISPPWM*10+1:                                //Display PWM Width
                 ret_mode = 'R';
                 PWM_Out(false);
-                i = conf.ESC_Low;
                 LCD_locate(0,0);
-                sprintf(str,"M1=%4d,M2=%4d",i+M1,i+M2);
+                sprintf(str,"M1=%4d,M2=%4d",M1,M2);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"M4=%4d,M3=%4d",i+M4,i+M3);
+                sprintf(str,"M4=%4d,M3=%4d",M4,M3);
                 LCD_printf(str);
                 break;
             case DISPPWM*10+2:                                //Display PWM Width
                 ret_mode = 'R';
                 PWM_Out(false);
-                i = conf.ESC_Low;
                 LCD_locate(0,0);
-                sprintf(str,"M5=%4d,M6=%4d",i+M5,i+M6);
+                sprintf(str,"M5=%4d,M6=%4d",M5,M6);
                 LCD_printf(str);
                 break;
 
@@ -426,7 +444,7 @@
             case PARMSET*10:    //パラメーター設定
                 LCD_printf("Parameter Set");
                 Set_Arrow(1);
-                hmax = 8;
+                hmax = 9;
                 break;
             case PARMSET*10+1:
                 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
@@ -453,12 +471,23 @@
                 Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
                 break;
             case PARMSET*10+5:
-                Param_Set_Prompt1("ESC>Low Position",&conf.ESC_Low,2,Pulse_Min,Pulse_Max,1,sw);
+                Param_Set_Prompt1("ESC>Throttl Trim",&conf.Throttl_Trim,2,Pulse_Min,Pulse_Max,1,sw);
                 break;
             case PARMSET*10+6:
+                Param_Set_Prompt1("ESC>ReversePoint",&conf.Reverse_Point,2,1000,2000,1,sw);
+                break;
+            case PARMSET*10+7:
                 Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
                 break;
-            case PARMSET*10+7:
+            case PARMSET*10+8:
+                Param_Set_Prompt1("Thro Limit val",&conf.Thro_Limit_Val,2,0,200,1,sw);
+                throLimit.differential(conf.Thro_Limit_Val);
+                break;
+            case PARMSET*10+9:
+                Param_Set_Prompt1("Thro Limit Rate",&conf.Thro_Limit_Rate,3,0,1,0.01,sw);
+                throLimit.rate(conf.Thro_Limit_Rate);
+                break;
+/*            case PARMSET*10+7:
                 LCD_locate(0,0);
                 LCD_printf("Model Type");
                 LCD_locate(0,1);
@@ -475,9 +504,9 @@
                 Set_Arrow(2);
                 break;
             case PARMSET*10+8:
-                Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Jyro_Gain,3,0.0f,1.0f,0.01f,sw);
+                Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
                 break;
-
+*/
             //設定データの保存
             case CONFSTORE*10:       //E2PROM Store
                 LCD_printf("Config Save");
@@ -547,6 +576,10 @@
                 }
                 break;
             case 'E':
+                while ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) {
+                    FlashLED(2);
+                    wait(0.5);
+                }
                 LCD_cls();                 //Clear LCD
                 LCD_locate(0,0);
                 LCD_printf("PWM Started");
@@ -676,3 +709,4 @@
             LCD_printf("  <<");
     }
 };
+

--- a/config.h	Fri Oct 17 03:56:56 2014 +0000
+++ b/config.h	Tue Feb 24 09:28:29 2015 +0000
@@ -34,7 +34,7 @@
 #define TICK_TIME 0.05
 #define GYRO_ADJUST 2
 
-enum PlaneType{Quad_X=0,Quad_H,Delta,Delta_TW,AirPlane};
+enum PlaneType{Quad_X=0,Quad_VP,Quad_3D,Delta,Delta_TW,AirPlane};
 enum JR{_THR=0,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4};       //JR 信号の順番
 //enum Futaba{_AIL=0,_ELE,_THR,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4};       //Futaba 信号の順番
 enum stknum{ROL=0,PIT,YAW,COL,GAIN,AUX2};       // Stick[]の順番
@@ -53,7 +53,7 @@
     char StartMode;              //'c':config mode  'f':flight mode
     char Model_Type;             // 0x00:Quad-X  0x01:Quad-H-3D
     signed char Gyro_Gain_Setting;
-    int Stick_Ref[5];            //Stick Neutral Position
+    int Stick_Ref[6];            //Stick Neutral Position
     float Stick_Mix[3];               //Mixing ratio of stick operation
     signed char Gyro_Dir[4];
     float Gyro_Gain[6];
@@ -65,22 +65,26 @@
     int Flight_Time;
     int LCD_Contrast;
     int PWM_Mode;
-    int ESC_Low;
+    int Throttl_Trim;
     int PWM_Interval; 
 //    pidinf pid[4];
-    float Active_Jyro_Gain;
+    float Active_Gyro_Gain;
 //    float Gimbal_Gain;
 //    int Gimbal_Neutral_Width;
 //    int Gimbal_Dir;
+    int Reverse_Point;
+    float Thro_Limit_Val;
+    float Thro_Limit_Rate;
 public:
     config() {
-        Revision = 2.10;
+        Revision = 3.10;
         Struct_Size = sizeof(config);
         Stick_Ref[0] = 1500;
         Stick_Ref[1] = 1500;
         Stick_Ref[2] = 1500;
-        Stick_Ref[3] = 1100;
+        Stick_Ref[3] = 1080;
         Stick_Ref[4] = 1500;
+        Stick_Ref[5] = 1500;
         Stick_Mix[0] = 0.4;
         Stick_Mix[1] = 0.4;
         Stick_Mix[2] = 0.65;
@@ -98,6 +102,8 @@
         Servo_Dir[1] = 1;
         Servo_Dir[2] = 1;
         Servo_Dir[3] = 1;
+        Servo_Dir[4] = 1;
+        Servo_Dir[5] = 1;
         Accel_Ref[0] = 0;
         Accel_Ref[1] = 0;
         Accel_Ref[2] = 0;
@@ -110,11 +116,14 @@
         Flight_Time=360;
         LCD_Contrast = 60;
         PWM_Mode = 1;
-        ESC_Low= 1080;
-        PWM_Interval = 5000; 
+        Throttl_Trim = 0;
+        PWM_Interval = 3000; 
         StartMode = 'C';
-        PID_Interval = 0.005;
-        Active_Jyro_Gain = 0.5; 
+        PID_Interval = 0.004;
+        Active_Gyro_Gain = 0.6; 
+        Reverse_Point = 1500;
+        Thro_Limit_Val = 100;
+        Thro_Limit_Rate = 0.2;
     }
 };
 #endif
@@ -169,3 +178,5 @@
 
 
 
+
+

--- a/main.cpp	Fri Oct 17 03:56:56 2014 +0000
+++ b/main.cpp	Tue Feb 24 09:28:29 2015 +0000
@@ -3,13 +3,14 @@
     ------------+-----------+-----------+-----------+-----------+---------------------
     Quad-X      |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC |   -       |   -  
     Quad-H      |FL VP Ser  |FR VP Ser  |BL VP Ser  |BR VP Ser  |Thr ESC    |   -
-    Delta       |FL Tro ESC |L Ale Ser  |R Ale Ser  |Rud Ser    |   -       |   -     
-    Delta-TW    |FL Tro ESC |L Ale Ser  |R Ale Ser  |Rud Ser    |FR Thr ESC |   - 
+    Quad-3D     |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC |   -       |   -  
+    Delta       |FL Tro ESC |L Alv Ser  |R Alv Ser  |Rud Ser    |   -       |   -     
+    Delta-TW    |FL Tro ESC |L Alv Ser  |R Alv Ser  |Rud Ser    |FR Thr ESC |   - 
     Airplane    |Thr ESC    |Ail Ser    |Ele Ser    |Rud Ser    |Ail Ser    |   -
     
-    Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Ale:Alevon
+    F:Front,  B:Back,  L:Left,  R:Right
+    Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon
     ESC:for ESC, Ser:for Servo, VP:Variable Pitch
-    F:Front,  B:Back,  L:Left,  R:Right
 */
 
 #include "mbed.h"
@@ -33,7 +34,7 @@
 #if defined(TARGET_LPC1768)
     DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
 //    #ifdef LPCXpresso
-//        #define LED1 P0_22
+        #define LED1 P0_22
 //    #endif
     DigitalOut led1(LED1);
 //    DigitalOut led2(LED2);
@@ -101,6 +102,7 @@
 Timer FlyghtTime;
 config conf;
 //PID pid[4];
+Limiter throLimit = 100;
 Limiter gyroLimit[3] = {300,300,300};
 Limiter accLimit[3] = {0.5,0.5,0.5};
 Limiter pwmLimit[4] = {50,50,50,50};
@@ -119,12 +121,20 @@
 volatile float Gyro_Save[3];
 volatile int Stick[6];
 volatile int Stick_Save[6];
-int PWM_Init[5][6] = {  1080,1080,1080,1080,1080,1080,      //Quad_X
-                        1500,1500,1500,1500,1080,1080,      //Quad_H
+int PWM_Init[6][6] = {  1080,1080,1080,1080,1080,1080,      //Quad_X
+                        1500,1500,1500,1500,1080,1080,      //Quad_VP
+                        1500,1500,1500,1500,1080,1080,      //Quad_3D
                         1080,1500,1500,1500,1500,1080,      //Delta
                         1080,1500,1500,1500,1080,1500,      //Delta_TW
                         1080,1500,1500,1500,1500,1080      //AirPlane
                         };
+char Servo_idx[6][6] = { 0,0,0,0,0,0,      //Quad_X
+                         1,1,1,1,0,0,      //Quad_VP
+                         0,0,0,0,0,0,      //Quad_3D
+                         0,1,1,1,1,1,      //Delta
+                         0,1,1,1,0,1,      //Delta_TW
+                         0,1,1,1,1,1      //AirPlane
+                        };
 //int Stick_Max[3];
 float Press;
 float Base_Press;
@@ -137,8 +147,8 @@
 int loop_cnt;
 float target_height;
 float cuurent_height;
-float base_throttol;
-int throttol;
+float base_Throttl;
+int Throttl;
 bool hov_control;
 float Rdata;
 
@@ -162,7 +172,6 @@
 void LCD_printf(char *);
 void LCD_cls();
 void LCD_locate(int,int);
-void Servo_Reverse(int,int);
 #ifdef SOFTPWM
 void Tpwm_interrupt()
 {
@@ -197,7 +206,8 @@
     initialize();
 
     Get_Stick_Pos();
-    while (  Stick[COL] > Thro_Zero || conf.StartMode == 'C' )          //Shrottol Low
+    while (  Stick[COL] > Thro_Zero || conf.StartMode == 'C'
+             || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) )          //Shrottol Low
     {
 //        if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit )              // Shrottle High
 //            ESC_SetUp();
@@ -207,8 +217,8 @@
             SetUpPrompt(conf,i2c);
             break;
         }
-        FlashLED(3);
-        wait(0.3);
+        FlashLED(2);
+        wait(0.5);
         Get_Stick_Pos();
     }
     Flight_SetUp();
@@ -263,6 +273,8 @@
     else InPulseMode = 'P';
     led1 = 0;
     CycleTime.start();
+    throLimit.differential(conf.Thro_Limit_Val);
+    throLimit.rate(conf.Thro_Limit_Rate);
 #ifdef SOFTPWM
     Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
 #endif
@@ -439,13 +451,9 @@
     if ( err == false ) return false;
 //pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z);
 
-    x -= conf.Accel_Ref[0];
-    y -= conf.Accel_Ref[1];
-    z -= conf.Accel_Ref[2];
-
-    Accel[ROL] = accLimit[0].calc(x);
-    Accel[PIT] = accLimit[1].calc(y);
-    Accel[YAW] = accLimit[2].calc(z);
+    x = accLimit[0].calc(x) - conf.Accel_Ref[0];
+    y = accLimit[1].calc(y) - conf.Accel_Ref[1];
+    z = accLimit[2].calc(z) - conf.Accel_Ref[2];
 //pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]);
     Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f;
     Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f;
@@ -456,7 +464,7 @@
 void Get_Angle(float interval)
 {
     float x,y,z;
-    Get_Accel(interval);
+//    Get_Accel(interval);
     Get_Gyro(interval);
 
     x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f;
@@ -531,7 +539,7 @@
     
     interval = CycleTime.read();
     CycleTime.reset();
-    if ( interval > 0.2f ) return;
+    if ( interval > 0.005f && mode ) return;
     TotalTime += interval;
     if ( TotalTime > 0.5f )    {
         led1 = !led1; 
@@ -548,22 +556,27 @@
     Get_Stick_Pos();
     switch ( conf.Model_Type )   {
         case Quad_X:
-            M1 = M2 = M3 = M4 = Stick[COL];
+            M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim;
             break;
-        case Quad_H:
-            M1 = M2 = M3 = M4 = Stick[AUX2];
-            M5 = Stick[COL];
+        case Quad_VP:
+            M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL];
+            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;
             break;
         case Delta:
         case Delta_TW:
         case AirPlane:
-            M1 = Stick[COL];
-            M2 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL];
-            M3 = conf.Stick_Ref[PIT] - conf.Stick_Ref[COL];
-            M4 = conf.Stick_Ref[YAW] - conf.Stick_Ref[COL];
+            M1 = THR + conf.Throttl_Trim;
+            M2 = conf.Stick_Ref[ROL];
+            M3 = conf.Stick_Ref[PIT];
+            M4 = conf.Stick_Ref[YAW];
             if ( conf.Model_Type == AirPlane )
-                M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL];
-            else M5 = Stick[COL];
+                M5 = conf.Stick_Ref[ROL];
+            else M5 = M1;
             break;
     }
           
@@ -571,11 +584,12 @@
             
 //      Stick Angle Mixing
         if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
-        else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
-        if ( fabsf(Stick[i]) > 0 ) gain -= gain * ( fabsf(Stick[i]) / 500 ) * conf.Active_Jyro_Gain; 
+        else gain = ( (float)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
+        if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain;
         if ( Stick[GAIN] > 0 
             || i == YAW
             || conf.Model_Type > 0
+            || i2c.GetAddr(ACCEL_ADDR) == 0
             )  {
             reg[i] = Stick[i] * conf.Stick_Mix[i];
             reg[i] += Gyro[i] * gain * GYRO_ADJUST;
@@ -589,9 +603,14 @@
     }
     pid_interval = 0;
     
+//    int Reverse = 1;
     switch ( conf.Model_Type )  {
-    case Quad_X:
-    case Quad_H:
+      case Quad_3D:
+//        if ( THR < conf.Reverse_Point )  { 
+//            Reverse = -1;
+//        }
+      case Quad_X:
+      case Quad_VP:
        //Calculate Roll Pulse Width
         M1 += reg[ROL];
         M2 -= reg[ROL];
@@ -610,8 +629,8 @@
         M3 -= reg[YAW];
         M4 += reg[YAW];
         break;
-    case Delta:
-    case Delta_TW:    
+      case Delta:
+      case Delta_TW:    
         //Calculate Roll Pulse Width
         M2 += reg[ROL];
         M3 -= reg[ROL];
@@ -626,58 +645,52 @@
             M5 -= reg[YAW];
         }
         break;
-    case AirPlane:
-    //Calculate Roll Pulse Width
+      case AirPlane:
+        //Calculate Roll Pulse Width
         M2 -= reg[ROL];
         M5 -= reg[ROL];
-    //Calculate Pitch Pulse Width
+        //Calculate Pitch Pulse Width
         M3 += reg[PIT];
         //Calculate Yaw Pulse Width
         M4 -= reg[YAW];
         break;
     }
-    if ( conf.Model_Type != AirPlane )  {
-       for ( i=0;i<4;i++ )
+    int h = conf.Model_Type;
+    for ( int 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 ( M[i] > Thro_Hi )   M[i] = Thro_Hi; 
-            if ( M[i] < Thro_Lo )   M[i] = Thro_Lo;     // motor idle level
+            M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] );
+        }
+        else    {
+            if ( h == 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];
+            }
         }
     }
-    
-    switch ( conf.Model_Type )   {
-    case Quad_X:
-        if ( Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
-        break;
-    case Quad_H:
-        if ( Stick[COL] < Thro_Zero ) M5=0;
-        Servo_Reverse(1,4);
-        break;
-    case Delta:
-    case Delta_TW:
-        if ( Stick[COL] < Thro_Zero ) M1=M5=0;
-        Servo_Reverse(2,4);
-        break;
-    case AirPlane:
-        Servo_Reverse(2,5);
-        break;
-    }
     if ( mode ) {
+        h = conf.Stick_Ref[THR];
         for ( i=0;i<6;i++ ) {
 //           while ( !pwmpin[i] );
             if ( conf.PWM_Mode == 1 )
-                pwm[i].pulsewidth_us(conf.ESC_Low+M[i]);
-            else pwm[i].pulsewidth_us(M[i]);
+                pwm[i].pulsewidth_us(M[i]);
+            else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]);
         }
     }
         
 }
 
-void Servo_Reverse(int start,int end)   {
-    for ( int i=start-1; i<end; i++ ) {
-        if ( conf.Servo_Dir[i] == -1 ) 
-            M[i] = 1500 + ( ( conf.Stick_Ref[COL] + M[i] - 1500 ) * conf.Servo_Dir[i] ) - conf.Stick_Ref[COL];
-    }
-}
         
 void ESC_SetUp(void)    {
     while(1)    {
@@ -701,6 +714,8 @@
         pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
     }
     hov_control = false;
+    throLimit.differential(conf.Thro_Limit_Val);
+    throLimit.rate(conf.Thro_Limit_Rate);
     Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
     loop_cnt = 0;
     FlyghtTime.start();