Revision 3:27407c4984cf, committed 2014-02-13

Comitter:

komaida424

Date:

Thu Feb 13 16:07:07 2014 +0000

Parent:

2:59ac9df97701

Child:

4:4060309b9cc0

Commit message:

revsion.1.30

Changed in this revision

--- a/I2cPeripherals/I2cPeripherals.cpp	Fri Nov 15 20:53:36 2013 +0000
+++ b/I2cPeripherals/I2cPeripherals.cpp	Thu Feb 13 16:07:07 2014 +0000
@@ -4,7 +4,7 @@
 I2cPeripherals::I2cPeripherals(PinName sda, PinName scl): _i2c(sda,scl)
 {
 
-    _i2c.frequency(400000);
+    _i2c.frequency(200000);
 //    LCD_contrast = 60;
     wait(0.5);
     LCD_addr = 0;
@@ -125,7 +125,7 @@
                 _i2c.write(sens[num].addr,tx,2);
                 wait(0.01);
                 tx[0] = 0x1C;
-                tx[1] = 0x18;       // +-16g
+                tx[1] = 0x18;       // 00:2g,08:4g,10:8g,18:+-16g
                 _i2c.write(sens[num].addr,tx,2);
                 wait_ms(10);
                 break;
@@ -272,6 +272,7 @@
     //AXDL345 Data Read
     char rx[6];
     char tx[1];
+    float lsb;
     tx[0] = Accel_data | 0x80;
     if ( _i2c.write(Accel_addr,tx,1,true) != 0 )     {
         *x=*y=0;
@@ -283,9 +284,11 @@
         case 0xD0:
         case 0xD2:
 #ifdef MPU6050
-            *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * 0.0048;
-            *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * 0.0048;
-            *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * 0.0048;
+            lsb = 0.000488;     //16g
+//            lsb = 0.000122;       //4g
+            *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;
+            *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * lsb;
+            *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * lsb;
 #endif
             break;
         case 0xA6:
@@ -294,6 +297,7 @@
             *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * 0.04;
             *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * 0.04;
             *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * 0.04;
+            break;
 #endif
     }
     return true;

--- a/IAP/IAP.cpp	Fri Nov 15 20:53:36 2013 +0000
+++ b/IAP/IAP.cpp	Thu Feb 13 16:07:07 2014 +0000
@@ -30,7 +30,7 @@
 #define     USER_FLASH_AREA_START_STR( x )      STR( x )
 #define     STR( x )                            #x
 
-//unsigned char user_area[ USER_FLASH_AREA_SIZE ] __attribute__((section( ".ARM.__at_" USER_FLASH_AREA_START_STR( USER_FLASH_AREA_START ) ), zero_init));
+unsigned char user_area[ USER_FLASH_AREA_SIZE ] __attribute__((section( ".ARM.__at_" USER_FLASH_AREA_START_STR( USER_FLASH_AREA_START ) ), zero_init));
 
 
 /*

--- a/PID/PID.cpp	Fri Nov 15 20:53:36 2013 +0000
+++ b/PID/PID.cpp	Thu Feb 13 16:07:07 2014 +0000
@@ -5,7 +5,7 @@
 PID::PID()
 {   
     interval = 0.05;
-    integral_limit = 50;
+    differential_limit = 200;
     pid_limit = 300;
     kp = 0.5;
     ki = 0.5;
@@ -14,11 +14,11 @@
     old = 0;
 }
 
-void PID::init(float KP,float KI,float KD,float PID_LIM,float INTEGRAL_LIM)
+void PID::init(float KP,float KI,float KD,float PID_LIM,float DIFFERENTIAL_LIM)
 {   
 //    interval = INTERVAL;
     pid_limit = PID_LIM;
-    integral_limit = INTEGRAL_LIM;
+    differential_limit = DIFFERENTIAL_LIM;
     kp = KP;
     ki = KI;
     kd = KD;
@@ -28,13 +28,17 @@
 
 float PID::calc(float value,float target,float interval) 
 {
+    float differential;
     cur = value - target;
 //    if ( (old >= 0 && cur <= 0) || (old <= 0 && cur >= 0) || fabsf(cur) < 1 )
 //        integral = 0;
     integral += ( cur + old ) * interval * 0.5f;
-    if ( integral < -integral_limit ) integral = -integral_limit;
-    if ( integral > integral_limit ) integral = integral_limit;
-    float pid = kp * cur + ki * integral + kd * ( cur - old ) / interval;
+//    if ( integral < -differential_limit ) integral = -differential_limit;
+//    if ( integral > integral_limit ) integral = integral_limit;
+    differential = ( kd * ( cur - old ) / interval );
+    if ( differential < -differential_limit ) differential = -differential_limit;
+    if ( differential > differential_limit ) differential = differential_limit;
+    float pid = ( kp * cur ) + ( ki * integral ) + differential;
     if ( pid < -pid_limit ) pid = -pid_limit;
     if ( pid > pid_limit ) pid = pid_limit;
     old = cur;

--- a/PID/PID.h	Fri Nov 15 20:53:36 2013 +0000
+++ b/PID/PID.h	Thu Feb 13 16:07:07 2014 +0000
@@ -16,7 +16,7 @@
     float interval;
     float cur,old;
     float kp,ki,kd;
-    float integral_limit;
+    float differential_limit;
     float pid_limit;
 };
 #endif
\ No newline at end of file

--- a/config.cpp	Fri Nov 15 20:53:36 2013 +0000
+++ b/config.cpp	Thu Feb 13 16:07:07 2014 +0000
@@ -92,11 +92,13 @@
                 break;
             case CALIBURATE+1:        //Calibrate Transmitter
                 LCD_printf("Start Calibrate");
+                wait(1);
                 for(i=0; i<4; i++)  {
                     conf.Stick_Ref[i] = 0;
                 }
                 for(i=0; i<16; i++) {
-                    wait(0.3);
+                    wait(0.03);
+                    Get_Stick_Pos();                   
                     conf.Stick_Ref[ROL] += AIL;
                     conf.Stick_Ref[PIT] += ELE;
                     conf.Stick_Ref[YAW] += RUD;
@@ -193,21 +195,21 @@
                 hmax = 3;
                 break;
             case ACCELCORRECT+1:
-                Param_Set_Prompt1("Accel>ROL",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.01f,sw);
+                Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw);
                 break;
             case ACCELCORRECT+2:
-                Param_Set_Prompt1("Accel>PIT",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.01f,sw);
+                Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw);
                 break;
             case ACCELCORRECT+3:
-                Param_Set_Prompt1("Accel>YAW",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.01f,sw);
+                Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.001f,sw);
                 break;
             case PIDSET:        //PID Setting
                 LCD_printf("PID Setting");
                 Set_Arrow(1);
-                hmax = 8;
+                hmax = 9;
                 break;
             case PIDSET+1:
-                Param_Set_Prompt1("PID>RoolPitch>kp",&conf.kp[0],2,0.00f,5.00f,0.01f,sw);
+                Param_Set_Prompt1("PID>RoolPitch>kp",&conf.kp[0],2,0.00f,15.00f,0.01f,sw);
                 conf.kp[1] = conf.kp[0];
                 break;
             case PIDSET+2:                               
@@ -219,7 +221,7 @@
                 conf.kd[1] = conf.kd[0];
                 break;
             case PIDSET+4:
-                Param_Set_Prompt1("PID>YAW>kp",&conf.kp[2],2,0.00f,5.00f,0.01f,sw);
+                Param_Set_Prompt1("PID>YAW>kp",&conf.kp[2],2,0.00f,15.00f,0.01f,sw);
                 break;
             case PIDSET+5:                               
                 Param_Set_Prompt1("PID>YAW>ki",&conf.ki[2],2,0.00f,5.00f,0.01f,sw);
@@ -228,10 +230,13 @@
                 Param_Set_Prompt1("PID>YAW>kd",&conf.kd[2],2,0.00f,5.00f,0.01f,sw);
                 break;
             case PIDSET+7:                                
+                Param_Set_Prompt1("PID Interval",&conf.PID_Interval,2,0.002f,0.015f,0.001f,sw);
+                break;
+            case PIDSET+8:                                
                 Param_Set_Prompt1("PID Limit",&conf.PID_Limit,2,0.00f,400.00f,10.00f,sw);
                 break;
-            case PIDSET+8:                                
-                Param_Set_Prompt1("Integral Limit",&conf.Integral_Limit,3,0.00f,100.00f,1.00f,sw);
+            case PIDSET+9:                                
+                Param_Set_Prompt1("differential Lim",&conf.Differential_Limit,3,0.00f,300.00f,10.00f,sw);
                 break;
             case STICKMIX:        //Set Stick Mixing
                 LCD_printf("Set Stick Mixing");
@@ -278,32 +283,37 @@
                 hmax = 7;
                 for ( i=0; i<3; i++ )   {
                     pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])/50.0
-                            ,conf.PID_Limit,conf.Integral_Limit);
+                            ,conf.PID_Limit,conf.Differential_Limit);
                     Angle[i] = 0;
                 }
                 break;
             case DISPSENSOR+1:        //Gyro
-                Get_Gyro();
+//                Get_Gyro();
+                if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
+                else i2c.angular(&y,&x,&z);
+                x -= Gyro_Ref[0];
+                y -= Gyro_Ref[1];
+                z -= Gyro_Ref[2];
                 LCD_locate(0,0);
-                sprintf(str,"[Gyro]X=%5.1f",Gyro[ROL]);
+                sprintf(str,"[Gyro]X=%5.1f",x);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"y=%5.1f,Z=%5.1f",Gyro[PIT],Gyro[YAW]);
+                sprintf(str,"y=%5.1f,Z=%5.1f",y,z);
                 LCD_printf(str);
                 ret_mode = 'R';
                 break;
             case DISPSENSOR+2:            //Accelerometer
-                Get_Accel();
+//                Get_Accel();
                 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
                 else i2c.Acceleration(&y,&x,&z);
                 x -= conf.Accel_Ref[0];
                 y -= conf.Accel_Ref[1];
                 z -= conf.Accel_Ref[2];
                 LCD_locate(0,0);
-                sprintf(str,"[Accel]X=%5.1f",x);
+                sprintf(str,"[Accel]X=%5.2f",x);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"Y=%5.1f,Z=%5.1f",y,z);
+                sprintf(str,"Y=%5.2f,Z=%5.2f",y,z);
                 LCD_printf(str);
 //                Set_Arrow(2);
                 ret_mode = 'R';
@@ -336,7 +346,7 @@
                 Get_Pressure();
                 elaps.stop();
                 LCD_locate(0,0);
-                sprintf(str,"Press=%9.3f",Press/4096);
+                sprintf(str,"Press=%9.1f",Press/4096);
                 LCD_printf(str);
                 LCD_locate(0,1);
                 sprintf(str,"Elaps=%6d",elaps.read_us());
@@ -439,9 +449,9 @@
                 hmax = 3;
                 break;
             case CONFRESET+1:
-                LCD_printf("If want to reset");
+                LCD_printf("Ailron stick");
                 LCD_locate(0,1);
-                LCD_printf("Aileron right");
+                LCD_printf("Move to right");
                 Set_Arrow(2);
                 break;
             case CONFRESET+2:       //E2PROM reset
@@ -581,7 +591,7 @@
     LCD_locate(0,0);
     LCD_printf(hd);
     LCD_locate(0,1);
-    sprintf(str,"%7.2f",*num);
+    sprintf(str,"%7.3f",*num);
     LCD_printf(str);
     Set_Arrow(arrow);
     switch ( sw ) {
@@ -651,3 +661,11 @@
 
 
 
+
+
+
+
+
+
+
+

--- a/config.h	Fri Nov 15 20:53:36 2013 +0000
+++ b/config.h	Thu Feb 13 16:07:07 2014 +0000
@@ -27,7 +27,7 @@
 #define _ITG3200  0x00
 #define _L3GD20   0x01
 #define TICK_TIME 0.05
-#define GYRO_ADJUST 3
+#define GYRO_ADJUST 2
 
 //enum PortNum{Port_THR=1,Port_AIL=3,Port_ELE=5,Port_RUD=7,Port_AUX=6};
 enum IRNum{IR_THR=1,IR_AIL,IR_ELE,IR_RUD,IR_AUX};       //Input Pulse Sequence
@@ -45,7 +45,7 @@
     float Accel_Ref[3];
     float Accel_Gain[3];
     int Gyro_Gain_Setting;
-    float Control_Exchange_Angle;
+    float PID_Interval;
     float Cutoff_Freq;
     int Flight_Time;
     int LCD_Contrast;
@@ -60,10 +60,10 @@
     float ki[3];
     float kd[3];
     float PID_Limit;
-    float Integral_Limit;
+    float Differential_Limit;
 public:
     config() {
-        Revision = 1.25;
+        Revision = 1.30;
         Struct_Size = sizeof(config);
         Stick_Ref[0] = 1500;
         Stick_Ref[1] = 1500;
@@ -73,8 +73,8 @@
         Stick_Mix[0] = 0.3;
         Stick_Mix[1] = 0.3;
         Stick_Mix[2] = 0.65;
-        Gyro_Dir[0] = 1;
-        Gyro_Dir[1] = 1;
+        Gyro_Dir[0] = -1;
+        Gyro_Dir[1] = -1;
         Gyro_Dir[2] = 1;
         Gyro_Dir[3] = -1;
         Gyro_Gain[0] = 0.40;
@@ -98,8 +98,8 @@
         Flight_Time=360;
         LCD_Contrast = 60;
         PWM_Mode = 1;
-        ESC_Low= 1025;
-        PWM_Interval = 3000; 
+        ESC_Low= 1080;
+        PWM_Interval = 2200; 
 //        Accel_Rang = 2;
 //        Accel_Rate = 14;
 //        PWM_Resolution = 0;
@@ -108,15 +108,15 @@
         kp[0] = 1.5;
         kp[1] = 1.5;
         kp[2] = 1.5;
-        ki[0] = 0.7;
-        ki[1] = 0.7;
-        ki[2] = 0.7;
-        kd[0] = 1.2;
-        kd[1] = 1.2;
-        kd[2] = 1.2;
-        PID_Limit = 300;
-        Integral_Limit = 30;
-        Control_Exchange_Angle = 15;
+        ki[0] = 0.0;
+        ki[1] = 0.0;
+        ki[2] = 0.0;
+        kd[0] = 0.8;
+        kd[1] = 0.8;
+        kd[2] = 0.8;
+        PID_Limit = 350;
+        Differential_Limit = 200;
+        PID_Interval = 0.003;
     }
 };
 #endif
@@ -153,3 +153,10 @@
 
 
 
+
+
+
+
+
+
+

--- a/main.cpp	Fri Nov 15 20:53:36 2013 +0000
+++ b/main.cpp	Thu Feb 13 16:07:07 2014 +0000
@@ -35,13 +35,14 @@
     #define p6 dp9
     #define p7 dp10
     #define p8 dp11
-    #define p9 dp25
+    #define p9 dp13
     #define p10 dp26
     #define p13 dp16
     #define p21 dp1
     #define p22 dp2
     #define p23 dp18
     #define p24 dp24
+    #define p26 dp25
     #define p27 dp27
     #define p28 dp5
 #else                   //LPC1768
@@ -87,10 +88,11 @@
 int channel = 0;
 //int intrupt_cnt = 0;
 //int intrupt_cnt2 = 0;
-volatile int CH[5];
+int volatile CH[5];
 volatile int M[6];
 volatile float Gyro[3];
 volatile float Accel[3];
+volatile float Accel_Save[3];
 volatile float Angle[3];
 volatile float Gyro_Ref[3];
 volatile float Gyro_Save[3];
@@ -102,6 +104,7 @@
 //volatile bool tick_flag;
 //volatile bool buzz_flag;
 float interval;
+float pid_interval;
 int pid_reg[3];
 int loop_cnt;
 
@@ -113,8 +116,8 @@
 void Get_Stick_Pos();
 void CalibrateGyros(void);
 void CalibrateAccel(void);
-void Get_Gyro();
-bool Get_Accel();
+void Get_Gyro(float);
+bool Get_Accel(float);
 void Get_Angle(float);
 void ReadConfig();
 void WriteConfig();
@@ -171,7 +174,7 @@
 {
     int i=0;
     
-    wait(0.5);
+    wait(1.5);
     initialize();
 
     Get_Stick_Pos();
@@ -223,7 +226,7 @@
 
 void  initialize()
 {
-    buzz.period_us(500);
+    buzz.period_us(400);
     ReadConfig();               //config.inf file read
 
     i2c.start(conf.LCD_Contrast);
@@ -312,20 +315,25 @@
     char mem[MEM_SIZE];
     char *send;
     int i;
+//pc.printf("start\n\r");
     if ( sizeof(config) > 255 ) {
         LCD_printf("config size over");
         wait(3);
         return;
     }
+//pc.printf("iap start\n\r");
     send = (char*)&conf;
     for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
     for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
-    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
-    iap.erase( TARGET_SECTOR, TARGET_SECTOR );
-//pc.printf("erase\n");
-    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
-    iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
-//pc.printf("write\n");
+//pc.printf("prepare start\n\r");
+    int rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("prepare1(%5d)\n\r",rc);
+    rc = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("erase(%5d)\n\r",rc);
+    rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("prepare2(%5d)\n\r",rc);
+    rc = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+//pc.printf("write(%5d)\n\r",rc);
 #endif
 }
 
@@ -345,7 +353,7 @@
     Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
 }
 
-void Get_Gyro()
+void Get_Gyro(float interval)
 {
     float x,y,z;
     bool err;
@@ -359,43 +367,63 @@
     Gyro[ROL] = x - Gyro_Ref[0] ;
     Gyro[PIT] = y - Gyro_Ref[1] ;
     Gyro[YAW] = z - Gyro_Ref[2] ;
-    
+/*
+    x -= Gyro_Ref[0];
+    y -= Gyro_Ref[1];
+    z -= Gyro_Ref[2];
+    float i = 3.14 * 2 * 10 * interval;     LPF 10Hz
+    Gyro[ROL] += -Gyro[ROL] * i + x * i;
+    Gyro[PIT] += -Gyro[PIT] * i + y * i;
+    Gyro[YAW] += -Gyro[YAW] * i + z * i;
+*/    
     if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL];
     if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT];
     if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW];       
 }
 
-bool Get_Accel()
+bool Get_Accel(float interval)
 {
     float x,y,z;
-    bool err;
+    bool err;   
+    Accel_Save[ROL] = Accel[ROL];
+    Accel_Save[PIT] = Accel[PIT];
+    Accel_Save[YAW] = Accel[YAW];    
     if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
     else err=i2c.Acceleration(&y,&x,&z);
     if ( err == false ) return false;
-    float wx = x - conf.Accel_Ref[0];
-    float wy = y - conf.Accel_Ref[1];
-    float wz = z - conf.Accel_Ref[2];
-    Accel[ROL] = atan(wx/sqrt( pow(wy,2)+pow(wz,2)))*180/3.14;
-    Accel[PIT] = atan(wy/sqrt( pow(wx,2)+pow(wz,2)))*180/3.14;
-    Accel[YAW] = atan(sqrt( pow(wx,2)+pow(wy,2))/wz)*180/3.14;      
+//    Accel[ROL] = x - conf.Accel_Ref[0];
+//    Accel[PIT] = y - conf.Accel_Ref[1];
+//    Accel[YAW] = z - conf.Accel_Ref[2];
+
+    x -= conf.Accel_Ref[0];
+    y -= conf.Accel_Ref[1];
+    z -= conf.Accel_Ref[2];
+//    float i = 3.14 * 2 * 10 * interval;       //LPF 10Hz
+//    Accel[ROL] += -Accel[ROL] * i + x * i;
+//    Accel[PIT] += -Accel[PIT] * i + y * i;
+//    Accel[YAW] += -Accel[YAW] * i + z * i;
+
+    Accel[ROL] = atan(x/sqrt( pow(y,2)+pow(z,2)))*180/3.14;
+    Accel[PIT] = atan(y/sqrt( pow(x,2)+pow(z,2)))*180/3.14;
+//    Accel[YAW] = atan(sqrt( pow(x,2)+pow(y,2))/z)*180/3.14;      
     return true;  
 }
 
 void Get_Angle(float interval)
 {
-    Get_Gyro();
-    Get_Accel();
     float x,y,z;
+    Get_Accel(interval);
+    Get_Gyro(interval);
 
     x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5;
     y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5;
     z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5;
-    if ( Get_Accel() == true )  {
+    if ( Get_Accel(interval) == true )  {
         float i =  3.14 * 2 * conf.Cutoff_Freq * interval;
         Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
         Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
     }
-    else    {
+    else    {  
         Angle[ROL] += x * interval;
         Angle[PIT] += y * interval;
     }
@@ -442,7 +470,7 @@
     }
     conf.Accel_Ref[0] = k[0]/16;       
     conf.Accel_Ref[1] = k[1]/16;
-    conf.Accel_Ref[2] = k[2]/16-9.8;
+    conf.Accel_Ref[2] = k[2]/16-1;  
 //    FlashLED(3);
 }
 
@@ -452,11 +480,9 @@
     int i;
     float gain;
     
-    Get_Stick_Pos();
-    M1 = M2 = M3 = M4 = Stick[COL];
     interval = CycleTime.read();
     CycleTime.reset();
-    if ( interval > 0.1 ) return;
+    if ( interval > 0.2 ) return;
     TotalTime += interval;
     if ( TotalTime > 0.5 )    {
         led1 = !led1; 
@@ -466,37 +492,50 @@
     }
     
     Get_Angle(interval);
-    
+    pid_interval += interval;
+    if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return; 
+    Get_Stick_Pos();
+    M1 = M2 = M3 = M4 = Stick[COL];
+       
     for ( i=0;i<3;i++ ) {
             
 //      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 ( Stick[GAIN] > 0 
-//            || i == YAW 
-//            || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle 
+            || i == YAW 
             )  {
             reg[i] = Stick[i] * conf.Stick_Mix[i];
             reg[i] += Gyro[i] * gain * GYRO_ADJUST;
         }
         else   {
+//            reg[i] = Stick[i] * conf.Stick_Mix[i];
+//            reg[i] += pid[i].calc(Angle[i],0,pid_interval) * conf.Gyro_Dir[i];
+            float x = -(float)Stick[i]*45/400;
+            reg[i] = pid[i].calc(Angle[i],x*conf.Gyro_Dir[i],pid_interval) * conf.Gyro_Dir[i];
             if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) )   {
-                if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) )   {
-                    Stick_Save[i] = Stick[i]>>2;
+                if ( ( fabsf(Angle[i]) < fabsf(x) ) && ( fabsf(Angle[i]) > fabsf(x*0.7) ) ) { Angle[i] = 0; pid[i].reset(); }
+                if ( abs(Stick_Save[i]) <= abs(Stick[i]) )   {
+                    Stick_Save[i] = Stick[i];
                 }
                 else    {
-//                    pc.printf("PID RESET.%3d",i);wait(1);
-                    Stick_Save[i] = 0;
-                    pid[i].reset();
-                    Angle[i] = 0;
+                    if ( (abs(Stick_Save[i]) - abs(Stick[i])) > 10 ) {
+                        Stick_Save[i] = 0;
+                        pid[i].reset();
+                        Angle[i] = 0;
+                    }
                 }
             } 
-            reg[i] = Stick[i] * conf.Stick_Mix[i];
-            reg[i] += pid[i].calc(Angle[i],0,interval);
-//            reg[i] = pid[i].calc(Angle[i],-(float)Stick[i]*60/400,interval);
+//            else    {
+//                if ( fabsf(Accel[i]) < 0.001 )  {
+//                    pid[i].reset();
+//                    Angle[i] = 0;
+//                }
+//            }
         }
         pid_reg[i] = reg[i];
     }
+    pid_interval = 0;
     //Calculate Roll Pulse Width
     M1 += reg[ROL];
     M2 -= reg[ROL];
@@ -519,7 +558,7 @@
     {
 //        if ( Stick[COL] > 150 && M[i] < 150 )   M[i] = 150; 
         if ( M[i] > Thro_Hi )   M[i] = Thro_Hi; 
-        if ( M[i] < Thro_Lo )   M[i] = Thro_Lo;     // this is the motor idle level
+        if ( M[i] < Thro_Lo )   M[i] = Thro_Lo;     // motor idle level
     }
     
     if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
@@ -549,12 +588,13 @@
     for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(0);
     for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
     for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
-    for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])*0.02
-                                            ,conf.PID_Limit,conf.Integral_Limit);
+    for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]
+                                            ,conf.PID_Limit,conf.Differential_Limit);
     Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
     loop_cnt = 0;
     FlyghtTime.start();
     CycleTime.start();
+    pid_interval = 0;
     FlashLED(5);
 }
 
@@ -583,3 +623,5 @@
 
 
 
+
+

--- a/mbed.lib	Fri Nov 15 20:53:36 2013 +0000
+++ b/mbed.lib	Thu Feb 13 16:07:07 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/#f37f3b9c9f0b
+http://mbed.org/users/mbed_official/code/mbed/#673126e12c73