Revision 4:4060309b9cc0, committed 2014-10-14

Comitter:

komaida424

Date:

Tue Oct 14 08:15:03 2014 +0000

Parent:

3:27407c4984cf

Child:

5:7b02775787a9

Commit message:

rev.2.1

Changed in this revision

--- a/I2cPeripherals/I2cPeripherals.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/I2cPeripherals/I2cPeripherals.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -11,6 +11,9 @@
     Gyro_addr = 0;
     Accel_addr = 0;
     barometor_addr = 0;
+    ultrasonic_addr = 0;
+    ultrasonic_distance = 0;
+//pc.printf("start");
     find();
     start();
 
@@ -71,26 +74,28 @@
 
 void I2cPeripherals::find()
 {
+#define sensnum 11
     char tx[2];
     char rx[1];
-    SensorInf sens[10] = { 0xD0,0x68,0x00,      //ITG3200
+    SensorInf sens[sensnum] = { 0xD0,0x68,0x00,      //ITG3200   gyro
                            0xD2,0x68,0x00,
-                           0xD0,0x68,0x75,     //MPU6050
+                           0xD0,0x68,0x75,     //MPU6050    gyro/accel
                            0xD2,0x68,0x75,
-                           0xD4,0xD4,0x0F,    //L3GD20
+                           0xD4,0xD4,0x0F,    //L3GD20  gyro
                            0xD6,0xD4,0x0F,
-                           0xA6,0xE5,0x00,     //ADXL345
+                           0xA6,0xE5,0x00,     //ADXL345    accel
                            0x3A,0xE5,0x00,
-                           0xB8,0xBB,0x0F,     //LPS331
-                           0xBA,0xBB,0x0F
-                         };
+                           0xB8,0xBB,0x0F,     //LPS331 baro
+                           0xBA,0xBB,0x0F,
+                           0xE0,0x18,0x01       //SRF02  ultrasonic
+    };
 
-    for ( int num = 0; num < 10; num++ )    {
+    for ( int num = 0; num < sensnum; num++ )    {
         tx[0]= sens[num].whoaddr;
         if ( _i2c.write(sens[num].addr,tx,1,true) != 0 ) continue;
         _i2c.read (sens[num].addr,rx,1);
         if ( (rx[0]&0x7E) != (sens[num].who&0x7E) ) continue;
-
+//pc.printf("who=%2x",rx[0]);
         switch ( num )   {
 #ifdef ITG3200
             case 0:     //ITG3200
@@ -185,6 +190,14 @@
                 wait(1);
                 break;
 #endif
+            case 10:           //SFR02 ultrasonic
+                ultrasonic_addr = sens[num].addr;
+                ultrasonic_data = 0x02;
+                tx[0] = 0x00;
+                tx[1] = 0x52;
+                _i2c.write(sens[num].addr,tx,2);
+                wait(0.01);
+                break;
         }
     }
 }
@@ -205,7 +218,7 @@
     return -1;
 }
 
-void I2cPeripherals::write_reg(char I2cAddr,char reg_addr,char* data,int len)
+void I2cPeripherals::write_reg(int I2cAddr,char reg_addr,char* data,int len)
 {
     char tx[17];
     if ( len >16 ) len = 16;
@@ -215,19 +228,45 @@
     _i2c.write(I2cAddr,tx,len+1);
 }
 
-void I2cPeripherals::read_reg(char I2cAddr,char reg_addr,char* data,int len)
+void I2cPeripherals::read_reg(int I2cAddr,char reg_addr,char* data,int len)
 {
     char tx[1];
     tx[0] = reg_addr | 0x80;
     _i2c.write(I2cAddr,tx,1,true);
     _i2c.read(I2cAddr,data,len);
 }
+int I2cPeripherals::write_EEPROM(short reg_addr,char* data,int len)
+{
+    char tx[3];
+    int rc=0,i;
+    int I2cAddr = I2C_EEPROM_ADDR;
+    for (i=0; i<len; i++)  {
+        tx[0] = reg_addr >> 8;
+        tx[1] = reg_addr & 0xFF;
+        tx[2] = data[i];
+        rc = _i2c.write(I2cAddr,tx,3);
+        if ( rc ) break;
+        wait(0.01);
+        reg_addr ++;
+    }
+    return rc;  
+}
+
+int I2cPeripherals::read_EEPROM(short reg_addr,char* data,int len)
+{
+    int I2cAddr = I2C_EEPROM_ADDR;
+    int rc = _i2c.write(I2cAddr,(char*)& reg_addr,2,true);
+    if ( rc ) return rc;
+    rc = _i2c.read(I2cAddr,data,len);
+    return rc;
+}
 
 void I2cPeripherals::cls()
 {
     if ( LCD_addr == 0 ) return;
     char tx[2] = { 0x00, 0x01 };
     _i2c.write(LCD_addr,tx,2);
+    wait(0.001);
 }
 
 void I2cPeripherals::locate(int clm,int row)
@@ -238,7 +277,7 @@
     tx[0] = 0x00;
     tx[1] = 0x80 + (row * 0x40) + clm;
     _i2c.write(LCD_addr,tx,2);
-
+    wait(0.001);
 }
 
 int I2cPeripherals::angular(float *x,float *y,float *z)
@@ -276,7 +315,7 @@
     tx[0] = Accel_data | 0x80;
     if ( _i2c.write(Accel_addr,tx,1,true) != 0 )     {
         *x=*y=0;
-        *z=9.8;
+        *z=1;
         return false;
     }
     _i2c.read(Accel_addr,rx,6);
@@ -285,8 +324,10 @@
         case 0xD2:
 #ifdef MPU6050
             lsb = 0.000488;     //16g
-//            lsb = 0.000122;       //4g
-            *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;
+//            lsb = 0.000244;     //8g
+//            lsb = 0.000122;     //4g
+//            lsb = 0.000061;     //2g
+            *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;//re
             *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * lsb;
             *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * lsb;
 #endif
@@ -294,9 +335,10 @@
         case 0xA6:
         case 0x3A:
 #ifdef ADXL345
-            *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;
+            lsb = 0.004;
+            *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * lsb;//re
+            *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * lsb;
+            *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * lsb;
             break;
 #endif
     }
@@ -332,6 +374,40 @@
     return temp;
 }
 
+float I2cPeripherals::height_cm()
+{
+    return (float)ultrasonic(0x52)*0.0175f;
+}
+float I2cPeripherals::height_mm()
+{
+    return (float)ultrasonic(0x52)*0.175f;
+}
+
+float I2cPeripherals::height_us()
+{
+    return (float)ultrasonic(0x52)/1000000;
+}
+
+int I2cPeripherals::ultrasonic(char unit)
+{
+    char rx[2],tx[2];
+    tx[0] = 0x00;
+    if ( ultrasonic_addr == 0 ) return -1;
+    _i2c.write(ultrasonic_addr,tx,1,true);
+    _i2c.read (ultrasonic_addr,rx,1);
+    if ( rx[0] != 0xFF )    {
+        tx[0]= 0x02;
+        _i2c.write(ultrasonic_addr,tx,1,true);
+        _i2c.read (ultrasonic_addr,rx,2);
+        ultrasonic_distance = short(rx[0] << 8 | (uint8_t)rx[1]);
+        tx[0] = 0x00;
+        tx[1] = unit;
+        _i2c.write(ultrasonic_addr,tx,2);
+    }            
+    return ultrasonic_distance;
+
+}
+
 void I2cPeripherals::i2c_write(int i2caddr,char* tx,int len)
 {
     char rx[1];
@@ -355,6 +431,8 @@
         return LCD_addr;
     case BARO_ADDR:
         return barometor_addr;
+    case ULTRASONIC_ADDR:
+        return ultrasonic_addr;
     }
     return 0;        
 }
@@ -364,3 +442,6 @@
 
 
 
+
+
+

--- a/I2cPeripherals/I2cPeripherals.h	Thu Feb 13 16:07:07 2014 +0000
+++ b/I2cPeripherals/I2cPeripherals.h	Tue Oct 14 08:15:03 2014 +0000
@@ -15,15 +15,19 @@
 #define ACCEL_ADDR 1
 #define LCD_ADDR 3
 #define BARO_ADDR 4
+#define ULTRASONIC_ADDR 5
+#define I2C_EEPROM_ADDR 0xA0
 
 class I2cPeripherals : public Stream
 {
 public:
-    I2cPeripherals(PinName sda=p28, PinName scl=p27);
+    I2cPeripherals(PinName , PinName );
     
     void start(int contrast=60);
-    void write_reg(char,char,char*,int);
-    void read_reg(char,char,char*,int);
+    void write_reg(int,char,char*,int);
+    void read_reg(int,char,char*,int);
+    int write_EEPROM(short,char*,int);
+    int read_EEPROM(short,char*,int);
     void cls();
     void locate(int,int);
     int GetAddr(int);
@@ -31,11 +35,15 @@
     int Acceleration(float*,float*,float*);
     int pressure();
     int temperature();
+    float height_cm();
+    float height_mm();
+    float height_us();
 private:
     virtual int _putc(int value);
     virtual int _getc();
     void i2c_write(int,char*,int);
     void find();
+    int ultrasonic(char);
     I2C _i2c;
     int LCD_addr;
     char LCD_cmd;
@@ -46,6 +54,9 @@
     char Accel_data;
     int barometor_addr;
     char barometor_data;
+    int  ultrasonic_addr;
+    char  ultrasonic_data;
+    int ultrasonic_distance;
     int LCD_contrast;
     struct SensorInf    {
         char addr;

--- a/IAP/IAP.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/IAP/IAP.cpp	Tue Oct 14 08:15:03 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));
 
 
 /*
@@ -56,7 +56,7 @@
             IAPCommand_Compare,
             IAPCommand_Reinvoke_ISP,
             IAPCommand_Read_device_serial_number,
-#if defined(TARGET_LPC11U24)
+#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
             IAPCommand_EEPROM_Write = 61,
             IAPCommand_EEPROM_Read,
 #endif
@@ -228,7 +228,7 @@
     return ( USER_FLASH_AREA_SIZE );
 }
 
-#if defined(TARGET_LPC11U24)
+#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
 /** Copy RAM to EEPROM (LPC11U24)
  *  
  *  @param    source_addr    Source RAM address from which data bytes are to be read.
@@ -270,3 +270,5 @@
 }
 #endif
 
+
+

--- a/IAP/IAP.h	Thu Feb 13 16:07:07 2014 +0000
+++ b/IAP/IAP.h	Tue Oct 14 08:15:03 2014 +0000
@@ -29,7 +29,7 @@
 
 #include    "mbed.h"
 
-#if defined(TARGET_LPC1768)
+#if defined(TARGET_LPC1768)// || defined(TARGET_NUCLEO_F401RE)
 
 #define     USER_FLASH_AREA_START   FLASH_SECTOR_29
 //#define     USER_FLASH_AREA_SIZE    (FLASH_SECTOR_SIZE_16_TO_29 * 1)
@@ -148,6 +148,58 @@
     (char *)FLASH_SECTOR_29    
 };
 
+#elif defined(TARGET_NUCLEO_F401RE)         //dummy
+
+#define     FLASH_SECTOR_29      0x00078000
+#define     FLASH_SECTOR_SIZE_0_TO_15    ( 4 * 1024)
+#define     FLASH_SECTOR_SIZE_16_TO_29   (32 * 1024)
+#define     USER_FLASH_AREA_START   FLASH_SECTOR_29
+//#define     USER_FLASH_AREA_SIZE    (FLASH_SECTOR_SIZE_16_TO_29 * 1)
+#define     USER_FLASH_AREA_SIZE    (FLASH_SECTOR_SIZE_16_TO_29 * 1)
+
+
+#elif defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+
+#define     USER_FLASH_AREA_START   FLASH_SECTOR_15
+#define     USER_FLASH_AREA_SIZE    (FLASH_SECTOR_SIZE * 1)
+ 
+#define     FLASH_SECTOR_0       0x00000000
+#define     FLASH_SECTOR_1       0x00001000
+#define     FLASH_SECTOR_2       0x00002000
+#define     FLASH_SECTOR_3       0x00003000
+#define     FLASH_SECTOR_4       0x00004000
+#define     FLASH_SECTOR_5       0x00005000
+#define     FLASH_SECTOR_6       0x00006000
+#define     FLASH_SECTOR_7       0x00007000
+#define     FLASH_SECTOR_8       0x00008000
+#define     FLASH_SECTOR_9       0x00009000
+#define     FLASH_SECTOR_10      0x0000A000
+#define     FLASH_SECTOR_11      0x0000B000
+#define     FLASH_SECTOR_12      0x0000C000
+#define     FLASH_SECTOR_13      0x0000D000
+#define     FLASH_SECTOR_14      0x0000E000
+#define     FLASH_SECTOR_15      0x0000F000
+#define     FLASH_SECTOR_SIZE    (4 * 1024)
+
+static char * sector_start_adress[]    = {
+    (char *)FLASH_SECTOR_0,
+    (char *)FLASH_SECTOR_1,
+    (char *)FLASH_SECTOR_2,
+    (char *)FLASH_SECTOR_3,
+    (char *)FLASH_SECTOR_4,
+    (char *)FLASH_SECTOR_5,
+    (char *)FLASH_SECTOR_6,
+    (char *)FLASH_SECTOR_7,
+    (char *)FLASH_SECTOR_8,
+    (char *)FLASH_SECTOR_9,
+    (char *)FLASH_SECTOR_10,
+    (char *)FLASH_SECTOR_11,
+    (char *)FLASH_SECTOR_12,
+    (char *)FLASH_SECTOR_13,
+    (char *)FLASH_SECTOR_14,
+    (char *)FLASH_SECTOR_15
+};
+
 #elif defined(TARGET_LPC11U24) || defined(TARGET_LPC1114)
 
 #define     USER_FLASH_AREA_START   FLASH_SECTOR_7
@@ -269,7 +321,7 @@
     char *reserved_flash_area_start( void );
     int   reserved_flash_area_size( void );
 
-#if defined(TARGET_LPC11U24)
+#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
     int write_eeprom( char *source_addr, char *target_addr, int size );
     int read_eeprom( char *source_addr, char *target_addr, int size );
 #endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Limiter/Limiter.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -0,0 +1,26 @@
+#include "mbed.h"
+#include "Limiter.h"
+
+Limiter::Limiter(float LIMIT)
+{
+    limit = LIMIT;   
+    last= 0;
+    skip = false;
+}
+
+float Limiter::calc(float now)
+{
+    if ( !skip ) { 
+        skip = true; 
+        return last = now;
+    }
+    float differential = ( now - last );
+    if ( differential < limit && differential > -limit ) last = now;
+    if ( differential > limit ) last += limit * 0.05f;
+    else    {
+        if ( differential < -limit ) last -= limit * 0.05f;
+        else last = now;
+    }
+    return last;
+}
+;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Limiter/Limiter.h	Tue Oct 14 08:15:03 2014 +0000
@@ -0,0 +1,17 @@
+#ifndef MBED_LIMITER_H
+#define MBED_LIMITER_H
+
+#include "mbed.h"
+#include "stdarg.h"
+
+class Limiter
+{
+public:
+    Limiter(float limit);
+    float calc(float now);
+private:
+    float last;
+    float limit;
+    bool skip;
+};
+#endif
\ No newline at end of file

--- a/PID/PID.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-#include "mbed.h"
-#include "SerialLcd.h"
-#include "PID.h"
-
-PID::PID()
-{   
-    interval = 0.05;
-    differential_limit = 200;
-    pid_limit = 300;
-    kp = 0.5;
-    ki = 0.5;
-    kd = 0.2;
-    integral = 0;
-    old = 0;
-}
-
-void PID::init(float KP,float KI,float KD,float PID_LIM,float DIFFERENTIAL_LIM)
-{   
-//    interval = INTERVAL;
-    pid_limit = PID_LIM;
-    differential_limit = DIFFERENTIAL_LIM;
-    kp = KP;
-    ki = KI;
-    kd = KD;
-    integral = 0;
-    old = 0;    
-}
-
-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 < -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;
-    return pid;
-}
-
-void PID::reset()
-{
-    integral = 0;
-    old = 0;
-}
-;
-
-
-

--- a/PID/PID.h	Thu Feb 13 16:07:07 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-#ifndef MBED_PID_H
-#define MBED_PID_H
-
-#include "mbed.h"
-#include "stdarg.h"
-
-class PID
-{
-public:
-    PID();
-    void init(float KP,float KI,float KD,float PID_LIM,float INTGRAL_LIM);
-    float calc(float value,float target,float interval);
-    void reset();
-private:
-    float integral;
-    float interval;
-    float cur,old;
-    float kp,ki,kd;
-    float differential_limit;
-    float pid_limit;
-};
-#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseOut/PulseOut.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "InterruptIn.h"
+#include "PulseOut.h"
+
+PulseOut::PulseOut(PinName _outpin,bool _positive) : pulse(_outpin)     //constructa  
+{
+    if ( _positive )
+        pulse = 0;
+    else
+        pulse = 1;
+    positive = _positive;
+    width = 0; 
+}
+
+void PulseOut::pulsewidth_us(int _width)
+{
+    width = _width;
+}
+ 
+void PulseOut::start()
+{ 
+    if ( width == 0 ) return;
+    _timeout.attach_us(this,&PulseOut::stop,width);
+    if ( positive )
+        pulse = 1;
+    else
+        pulse = 0;    
+}
+
+void PulseOut::period_us(int period)
+{
+    //dummy
+} 
+
+void PulseOut::stop()
+{
+    if ( positive )
+        pulse = 0;
+    else
+        pulse = 1;    
+    _timeout.detach();
+}
+;
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseOut/PulseOut.h	Tue Oct 14 08:15:03 2014 +0000
@@ -0,0 +1,23 @@
+#ifndef PULSEOUT_H
+#define PULSEOUT_H
+#define POSITIVE true
+#define NEGATIVE false
+
+#include "mbed.h"
+
+class PulseOut  
+{
+private:
+    Timeout _timeout;
+    void stop();
+    DigitalOut pulse;
+    bool positive;
+    int width;
+
+public:
+    PulseOut(PinName,bool mode=true); 
+    void pulsewidth_us(int);
+    void period_us(int);
+    void start();
+};
+#endif
\ No newline at end of file

--- a/SerialLcd/SerialLcd.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/SerialLcd/SerialLcd.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "SerialLcd.h"
 
-SerialLcd::SerialLcd(PinName TX): _lcd(TX,NC)
+SerialLcd::SerialLcd(PinName TX,PinName RX): _lcd(TX,RX)
 {   
 //    LCD_contrast = 60; 
 }
@@ -28,4 +28,4 @@
     _lcd.putc( 0x80 + (row * 0x40) + clm );
     wait(0.01);
 }
-;
\ No newline at end of file
+;

--- a/SerialLcd/SerialLcd.h	Thu Feb 13 16:07:07 2014 +0000
+++ b/SerialLcd/SerialLcd.h	Tue Oct 14 08:15:03 2014 +0000
@@ -7,7 +7,7 @@
 class SerialLcd : public Stream
 {
 public:
-    SerialLcd(PinName);
+    SerialLcd(PinName,PinName=NC);
     
     void cls();
     void locate(int,int);

--- a/SoftPWM/SoftPWM.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/SoftPWM/SoftPWM.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -16,16 +16,16 @@
 
 float SoftPWM::read()
 {
-    if ( width <= 0.0 ) return 0.0;
-    if ( width > 1.0 )  return 1.0;
+    if ( width <= 0.0f ) return 0.0;
+    if ( width > 1.0f )  return 1.0;
     return width / interval;    
 }
 
 void SoftPWM::write(float duty)
 {
     width = interval * duty;
-    if ( duty <= 0.0 ) width =  0.0;
-    if ( duty > 1.0 )  width =  interval;
+    if ( duty <= 0.0f ) width =  0.0;
+    if ( duty > 1.0f )  width =  interval;
 }
 
 void SoftPWM::start()
@@ -64,7 +64,7 @@
 void SoftPWM::pulsewidth(float _width)
 {
     width = _width;
-   if ( width < 0.0 ) width = 0.0;
+   if ( width < 0.0f ) width = 0.0;
 }
 
 void SoftPWM::pulsewidth_ms(int _width)

--- a/SoftPWM/SoftPWM.h	Thu Feb 13 16:07:07 2014 +0000
+++ b/SoftPWM/SoftPWM.h	Tue Oct 14 08:15:03 2014 +0000
@@ -30,14 +30,14 @@
     void period_us(int);
     void stop();
     operator float()  { 
-        if ( width <= 0.0 ) return 0.0;
-        if ( width > 1.0 )  return 1.0;
+        if ( width <= 0.0f ) return 0.0f;
+        if ( width > 1.0f )  return 1.0f;
         return width / interval;
     }
     SoftPWM& operator=(float duty)  {
         width = interval * duty;
-        if ( duty <= 0.0 ) width =  0.0;
-        if ( duty > 1.0 )  width =  interval;
+        if ( duty <= 0.0f ) width =  0.0f;
+        if ( duty > 1.0f )  width =  interval;
         return *this;
     }
                 

--- a/config.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/config.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -4,22 +4,27 @@
 #include "config.h"
 #include "PulseWidthCounter.h"
 #include "SerialLcd.h"
-#include "PID.h"
+//#include "PID.h"
 
 //Serial pc(USBTX, USBRX); 
 
-#define CALIBURATE 10
-#define GYROGAIN 20
-#define GYRODIR 30
-#define ACCELCORRECT 40
-#define PIDSET 50
-#define STICKMIX 60
-#define DISPPULSE 70
-#define DISPSENSOR 80
-#define DISPPWM 90 
-#define PARMSET 100
-#define CONFSTORE 110
-#define CONFRESET 120
+enum DispNum {  CALIBURATE=1,
+                GYROGAIN,
+                GYRODIR,
+                SERVODIR,
+//                ACCELGAIN,
+                ACCELCORRECT,
+//                PIDSET,
+//                PIDHEIGHT,
+//                GIMBAL,
+                STICKMIX,
+                DISPPULSE,
+                DISPSENSOR,
+                DISPPWM,
+                PARMSET,
+                CONFSTORE,
+                CONFRESET,
+                FINAL   };
 
 void FlashLED(int);
 char Check_Stick_Dir(char);
@@ -42,23 +47,27 @@
 
 Timer elaps;
 
-extern volatile int CH[5];
+extern volatile int CH[9];
 extern volatile int M[6];
 extern volatile float Gyro[3];
 extern volatile float Accel[3];
+extern volatile float Accel_Save[3];
+extern volatile float Accel_Angle[3];
 extern volatile float Angle[3];
 extern volatile float Gyro_Ref[3];
-extern volatile int Stick[5];
+extern volatile int Stick[6];
 extern volatile float Press;
 extern volatile float interval;
 //extern bool tick_flag;
-extern PID pid[3];
-extern int pid_reg[3];
+//extern PID pid[4];
+//extern PID height;
+//extern int pid_reg[4];
 const char steering[3][6]= {"Roll ","Pitch","Yaw  "};
-short mode;
+const char ModelName[5][9] = { "Quad-X  ","Quad-H  ","Delta   ","Delta-TW","AirPlane" };    
+int mode;//
 char sw,ret_mode;
-short vnum,hnum,vmax,hmax;
-short idx,i;
+int vnum,hnum,vmax,hmax;//
+int idx,i;//
 char str[33];
 config init;
 
@@ -69,7 +78,7 @@
     mode = 0;
     vnum = 0;
     hnum = 0;
-    vmax = 12;
+    vmax = FINAL - 1;
 
     while( 1 ) {
 //        FlashLED(1);
@@ -77,20 +86,26 @@
         mode = vnum * 10 + hnum;
 
         switch ( mode ) {
+            
+            //初期画面
             case 0:
                 LCD_locate(0,0);
-                sprintf(str,"Quad-X  Ver %4.2f",conf.Revision);
+                LCD_printf( (char*)ModelName[conf.Model_Type] );
+                LCD_locate(8,0);
+                sprintf(str,"Ver %4.2f",conf.Revision);
                 LCD_printf(str);
                 LCD_locate(4,1);
                 LCD_printf("By AZUKITEN");
                 hmax = 0;
                 break;
-            case CALIBURATE:        //Calibrate Transmitter
+
+            //送信機信号のキャリブレーション
+            case CALIBURATE*10:        //Calibrate Transmitter
                 LCD_printf("Calibrate");
                 Set_Arrow(1);
                 hmax = 1;
                 break;
-            case CALIBURATE+1:        //Calibrate Transmitter
+            case CALIBURATE*10+1:        //Calibrate Transmitter
                 LCD_printf("Start Calibrate");
                 wait(1);
                 for(i=0; i<4; i++)  {
@@ -108,6 +123,7 @@
                 for(i=0; i<4; i++) {
                     conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
                 }
+
                 CalibrateGyros();
                 CalibrateAccel();
                 LCD_cls();                 //Clear LCD
@@ -116,30 +132,32 @@
                 FlashLED(5);
                 hnum = 0;
                 break;
-            case GYROGAIN:        //Set Gyro Gain
+
+            //ジャイロ感度の設定
+            case GYROGAIN*10:        //Set Gyro Gain
                 LCD_printf("Set Gyro Gain");
                 Set_Arrow(1);
                 hmax = 4;
                 break;
-            case GYROGAIN+1:                                //Set Gyro Gain Roll
+            case GYROGAIN*10+1:                                //Set Gyro Gain Roll
                 if ( conf.Gyro_Gain_Setting == 1 )
                     Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw);
                 else
                     Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
                 break;
-            case GYROGAIN+2:
+            case GYROGAIN*10+2:
                 if ( conf.Gyro_Gain_Setting == 1 )
                     Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw);
                 else
                     Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw);
                 break;
-            case GYROGAIN+3:
+            case GYROGAIN*10+3:
                 if ( conf.Gyro_Gain_Setting == 1 )
                     Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw);
                 else
                     Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
                 break;
-            case GYROGAIN+4:
+            case GYROGAIN*10+4:
 //                ret_mode = 'R';
                 LCD_printf("GyroGain>setting");
                 LCD_locate(0,1);
@@ -154,18 +172,20 @@
                     LCD_printf("Transmitter");
                 Set_Arrow(3);
                 break;
-            case GYRODIR:        //Set Gyro Direction
+                
+            //ジャイロの効きの逆転                
+            case GYRODIR*10:        //Set Gyro Direction
                 LCD_printf("Gyro Direction");
                 Set_Arrow(1);
                 hmax = 4;
                 break;
-            case GYRODIR+1:                                //Set Gyro Direction Roll
-            case GYRODIR+2:
-            case GYRODIR+3:
-            case GYRODIR+4:
+            case GYRODIR*10+1:                                //Set Gyro Direction Roll
+            case GYRODIR*10+2:
+            case GYRODIR*10+3:
+            case GYRODIR*10+4:          //ｘｙ軸の入れ替え
 //              ret_mode = 'R';
-                idx = mode - (GYRODIR+1);
-                if ( mode == (GYRODIR+4) )
+                idx = mode - (GYRODIR*10+1);
+                if ( mode == (GYRODIR*10+4) )
                     LCD_printf("Gyro>Swap X-Y");
                 else {
                     LCD_printf("Gyro>Dir>");
@@ -182,82 +202,86 @@
                     LCD_printf("Normal ");
                 else
                     LCD_printf("Reverse");
-                if ( mode == (GYRODIR+4) )
+                if ( mode == (GYRODIR*10+4) )
+                    Set_Arrow(3);
+                else
+                    Set_Arrow(2);
+                break;
+
+            //サーボの向きの逆転
+            case SERVODIR*10:        //Set Servo Direction
+                LCD_printf("Servo Direction");
+                Set_Arrow(1);
+                hmax = 6;
+                break;
+            case SERVODIR*10+1:                                //Set Gyro Direction Roll
+            case SERVODIR*10+2:
+            case SERVODIR*10+3:
+            case SERVODIR*10+4:
+            case SERVODIR*10+5:
+            case SERVODIR*10+6:
+//              ret_mode = 'R';
+                idx = mode - (SERVODIR*10+1);
+                sprintf(str,"Servo>Dir>M%d",idx+1);
+                LCD_printf(str);
+                LCD_locate(0,1);
+                switch ( sw ) {
+                    case 'U':
+                    case 'D':
+                        conf.Servo_Dir[idx] *= -1;
+                }
+                if ( conf.Servo_Dir[idx] == 1 )
+                    LCD_printf("Normal ");
+                else
+                    LCD_printf("Reverse");
+                if ( mode == (SERVODIR*10+4) )
                     Set_Arrow(3);
                 else
                     Set_Arrow(2);
                 break;
-            case ACCELCORRECT:
+
+            //加速度計の水平レベルの校正
+            case ACCELCORRECT*10:
                 LCD_printf("Acceleration");
                 LCD_locate(2,1);
-                LCD_printf("correction");
+                LCD_printf("Trim");
                 Set_Arrow(1);
                 hmax = 3;
                 break;
-            case ACCELCORRECT+1:
+            case ACCELCORRECT*10+1:
                 Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw);
                 break;
-            case ACCELCORRECT+2:
+            case ACCELCORRECT*10+2:
                 Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw);
                 break;
-            case ACCELCORRECT+3:
+            case ACCELCORRECT*10+3:
                 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 = 9;
-                break;
-            case PIDSET+1:
-                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:                               
-                Param_Set_Prompt1("PID>RoolPitch>ki",&conf.ki[0],2,0.00f,5.00f,0.01f,sw);
-                conf.ki[1] = conf.ki[0];
-                break;
-            case PIDSET+3:                                
-                Param_Set_Prompt1("PID>RoolPitch>kd",&conf.kd[0],2,0.00f,5.00f,0.01f,sw);
-                conf.kd[1] = conf.kd[0];
-                break;
-            case PIDSET+4:
-                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);
-                break;
-            case PIDSET+6:                                
-                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+9:                                
-                Param_Set_Prompt1("differential Lim",&conf.Differential_Limit,3,0.00f,300.00f,10.00f,sw);
-                break;
-            case STICKMIX:        //Set Stick Mixing
+
+            //スティック操作量の設定
+            case STICKMIX*10:        //Set Stick Mixing
                 LCD_printf("Set Stick Mixing");
                 Set_Arrow(1);
                 hmax = 3;
                 break;
-            case STICKMIX+1:                                //Set Stick Mixing
+            case STICKMIX*10+1:                                //Set Stick Mixing
                 Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
                 break;
-            case STICKMIX+2:
+            case STICKMIX*10+2:
                 Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
                 break;
-            case STICKMIX+3:
+            case STICKMIX*10+3:
                 Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
                 break;
-            case DISPPULSE:        //Display Pulse Width
+
+            //送信機パルス長の表示
+            case DISPPULSE*10:        //Display Pulse Width
                 LCD_printf("Disp Pulse Width");
                 Set_Arrow(1);
-                hmax = 2;
+                hmax = 3;
+                x = conf.ESC_Low;
                 break;
-            case DISPPULSE+1:        //Display Pulse Width
+            case DISPPULSE*10+1:        //Display Pulse Width
 //           DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
                 ret_mode = 'R';
                 LCD_locate(0,0);
@@ -267,7 +291,17 @@
                 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
                 LCD_printf(str);
                 break;
-            case DISPPULSE+2:        //Display Stick Ref
+            case DISPPULSE*10+2:        //Display AUX,AX2
+                ret_mode = 'R';
+                Get_Stick_Pos();
+                LCD_locate(0,0);
+                sprintf(str,"A1=%4d,A2=%4d",AUX,AX2);
+                LCD_printf(str);
+                LCD_locate(0,1);
+                sprintf(str,"A3=%4d,A4=%4d",AX3,AX4);
+                LCD_printf(str);
+                break;
+            case DISPPULSE*10+3:        //Display Stick Ref
                 ret_mode = 'R';
                 Get_Stick_Pos();
                 LCD_locate(0,0);
@@ -277,17 +311,16 @@
                 sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
                 LCD_printf(str);
                 break;
-            case DISPSENSOR:        //Display Sensor Value
-                LCD_printf("Disp Senser");
+
+            //センサー値の表示
+            case DISPSENSOR*10:        //Display Sensor Value
+                LCD_printf("Disp Sensor");
                 Set_Arrow(1);
-                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.Differential_Limit);
-                    Angle[i] = 0;
-                }
+                hmax = 6;
+                Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
+                Accel[ROL]=Accel[PIT]=Accel[YAW]=0;
                 break;
-            case DISPSENSOR+1:        //Gyro
+            case DISPSENSOR*10+1:        //Gyro
 //                Get_Gyro();
                 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
                 else i2c.angular(&y,&x,&z);
@@ -302,15 +335,14 @@
                 LCD_printf(str);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+2:            //Accelerometer
-//                Get_Accel();
+            case DISPSENSOR*10+2:            //Gravity
                 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.2f",x);
+                sprintf(str,"[Gravity]X=%5.2f",x);
                 LCD_printf(str);
                 LCD_locate(0,1);
                 sprintf(str,"Y=%5.2f,Z=%5.2f",y,z);
@@ -318,7 +350,7 @@
 //                Set_Arrow(2);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+3:            //angle
+            case DISPSENSOR*10+3:            //angle
                 PWM_Out(false);
                 LCD_locate(0,0);
                 sprintf(str,"[Angle]X=%6.1f",Angle[ROL]);
@@ -329,33 +361,22 @@
 //                Set_Arrow(2);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+4:
-                PWM_Out(false);
-                LCD_locate(0,0);
-                sprintf(str,"t=%5d,a=%6.2f",-Stick[YAW]*45/400,Angle[YAW]);
-                LCD_printf(str);
-                LCD_locate(0,1);
-                sprintf(str,"pid=%5d",pid_reg[YAW]);
-                LCD_printf(str);
-                Set_Arrow(2);
-                ret_mode = 'R';
-                break;
-            case DISPSENSOR+5:                // Pressure
+            case DISPSENSOR*10+4:                // Pressure
                 elaps.reset();
                 elaps.start();
                 Get_Pressure();
                 elaps.stop();
                 LCD_locate(0,0);
-                sprintf(str,"Press=%9.1f",Press/4096);
+                sprintf(str,"Press=%4.1fhp",Press);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"Elaps=%6d",elaps.read_us());
+                sprintf(str,"Height=%7.2fcm",i2c.height_cm());
                 LCD_printf(str);
 //                Set_Arrow(2);
                 ret_mode = 'R';
                 wait(0.05);
                 break;
-            case DISPSENSOR+6:
+            case DISPSENSOR*10+5:
                 elaps.reset();
                 elaps.start();
                 PWM_Out(false);
@@ -364,39 +385,53 @@
                 LCD_locate(0,0);
                 sprintf(str,"ElapsTime=%6d",i);
                 LCD_printf(str);
-                Set_Arrow(2);
+//                Set_Arrow(2);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+7:        //Sensor Calibration
+            case DISPSENSOR*10+6:        //Sensor Calibration
                 CalibrateGyros();
                 FlashLED(3);
                 LCD_printf("Calibrate completed");
                 Set_Arrow(3);
                 break;
-            case DISPPWM:                                //Display PWM Condition
+
+
+            //ＥＳＣ用PWMパルス長の表示
+            case DISPPWM*10:                                //Display PWM Condition
                 LCD_printf("Display PWM ");
                 Set_Arrow(1);
-                hmax = 1;
+                hmax = 2;
                 break;
-            case DISPPWM+1:                                //Display PWM Width
+            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",M1,M2);
+                sprintf(str,"M1=%4d,M2=%4d",i+M1,i+M2);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"M4=%4d,M3=%4d",M4,M3);
+                sprintf(str,"M4=%4d,M3=%4d",i+M4,i+M3);
                 LCD_printf(str);
                 break;
-            case PARMSET:    //パラメーター設定
+            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);
+                LCD_printf(str);
+                break;
+
+            //その他パラメータ値の設定
+            case PARMSET*10:    //パラメーター設定
                 LCD_printf("Parameter Set");
                 Set_Arrow(1);
-                hmax = 6;
+                hmax = 8;
                 break;
-            case PARMSET+1:
+            case PARMSET*10+1:
                 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
                 break;
-            case PARMSET+2:
+            case PARMSET*10+2:
                 LCD_locate(0,0);
                 LCD_printf("PWM>Mode");
                 LCD_locate(0,1);
@@ -411,27 +446,45 @@
                     LCD_printf("Moter");
                 Set_Arrow(2);
                 break;
-            case PARMSET+3:
-                Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
+            case PARMSET*10+3:
+                Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,20000,100,sw);
                 break;
-            case PARMSET+4:                                
+            case PARMSET*10+4:                                
                 Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
                 break;
-            case PARMSET+5:
+            case PARMSET*10+5:
                 Param_Set_Prompt1("ESC>Low Position",&conf.ESC_Low,2,Pulse_Min,Pulse_Max,1,sw);
                 break;
-            case PARMSET+6:
+            case PARMSET*10+6:
                 Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
                 break;
-//            case PARMSET+7:
-//                Param_Set_Prompt1("Cont. swap Angle",&conf.Control_Exchange_Angle,3,0.0f,40.0f,1.0f,sw);
-//                break;
-            case CONFSTORE:       //E2PROM Store
+            case PARMSET*10+7:
+                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 = 3;
+                        break;
+                    case 'U':
+                        if ( conf.Model_Type < 4 ) conf.Model_Type += 1;
+                        else conf.Model_Type = 0;
+                }
+                LCD_printf( (char*)ModelName[conf.Model_Type] );
+                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);
+                break;
+
+            //設定データの保存
+            case CONFSTORE*10:       //E2PROM Store
                 LCD_printf("Config Save");
                 Set_Arrow(1);
                 hmax = 1;
                 break;
-            case CONFSTORE+1:
+            case CONFSTORE*10+1:
                 WriteConfig();
                 LCD_locate(0,0);
                 sprintf(str,"Config %3dbyte",sizeof(config));
@@ -443,18 +496,20 @@
                 FlashLED(5);
                 hnum = 0;
                 break;
-            case CONFRESET:       //E2PROM reset
+
+            //設定データの初期化
+            case CONFRESET*10:       //E2PROM reset
                 LCD_printf("Config Reset");
                 Set_Arrow(1);
                 hmax = 3;
                 break;
-            case CONFRESET+1:
+            case CONFRESET*10+1:
                 LCD_printf("Ailron stick");
                 LCD_locate(0,1);
                 LCD_printf("Move to right");
                 Set_Arrow(2);
                 break;
-            case CONFRESET+2:       //E2PROM reset
+            case CONFRESET*10+2:       //E2PROM reset
                 conf = init;
                 LCD_printf("Rset sucssesuful");
                 Set_Arrow(3);
@@ -620,52 +675,4 @@
         case 3:
             LCD_printf("  <<");
     }
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+};

--- a/config.h	Thu Feb 13 16:07:07 2014 +0000
+++ b/config.h	Tue Oct 14 08:15:03 2014 +0000
@@ -24,27 +24,42 @@
 #define ELE CH[2]
 #define RUD CH[3]
 #define AUX CH[4]
+#define AX2 CH[5]
+#define AX3 CH[6]
+#define AX4 CH[7]
+#define AX5 CH[8]
+#define HEIGHT 3
 #define _ITG3200  0x00
 #define _L3GD20   0x01
 #define TICK_TIME 0.05
 #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
-enum channel{ROL=0,PIT,YAW,COL,GAIN};
-
+enum PlaneType{Quad_X=0,Quad_H,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[]の順番
+/*
+struct pidinf {
+    float kp;
+    float ki;
+    float kd;
+    float limit;
+    float integral_limit;
+    float differential_limit;
+};*/
 struct config {
     float Revision;
     int Struct_Size;
     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
     float Stick_Mix[3];               //Mixing ratio of stick operation
     signed char Gyro_Dir[4];
     float Gyro_Gain[6];
-    signed char Accel_Dir[4];
+    signed char Servo_Dir[6];
     float Accel_Ref[3];
-    float Accel_Gain[3];
-    int Gyro_Gain_Setting;
+//    float Accel_Gain[3];
     float PID_Interval;
     float Cutoff_Freq;
     int Flight_Time;
@@ -52,71 +67,54 @@
     int PWM_Mode;
     int ESC_Low;
     int PWM_Interval; 
-//    int Accel_Rang;
-//    int Accel_Rate;
-//    int PWM_Resolution;
-    char Angle_Control;
-    float kp[3];
-    float ki[3];
-    float kd[3];
-    float PID_Limit;
-    float Differential_Limit;
+//    pidinf pid[4];
+    float Active_Jyro_Gain;
+//    float Gimbal_Gain;
+//    int Gimbal_Neutral_Width;
+//    int Gimbal_Dir;
 public:
     config() {
-        Revision = 1.30;
+        Revision = 2.10;
         Struct_Size = sizeof(config);
         Stick_Ref[0] = 1500;
         Stick_Ref[1] = 1500;
         Stick_Ref[2] = 1500;
         Stick_Ref[3] = 1100;
         Stick_Ref[4] = 1500;
-        Stick_Mix[0] = 0.3;
-        Stick_Mix[1] = 0.3;
+        Stick_Mix[0] = 0.4;
+        Stick_Mix[1] = 0.4;
         Stick_Mix[2] = 0.65;
         Gyro_Dir[0] = -1;
         Gyro_Dir[1] = -1;
         Gyro_Dir[2] = 1;
         Gyro_Dir[3] = -1;
-        Gyro_Gain[0] = 0.40;
-        Gyro_Gain[1] = 0.40;
-        Gyro_Gain[2] = 0.40;
+        Gyro_Gain[0] = 0.50;
+        Gyro_Gain[1] = 0.50;
+        Gyro_Gain[2] = 0.50;
         Gyro_Gain[3] = 0.00;
         Gyro_Gain[4] = 0.00;
         Gyro_Gain[5] = 0.00;
-        Accel_Dir[0] = 1;
-        Accel_Dir[1] = 1;
-        Accel_Dir[2] = 1;
-        Accel_Dir[3] = 1;
+        Servo_Dir[0] = 1;
+        Servo_Dir[1] = 1;
+        Servo_Dir[2] = 1;
+        Servo_Dir[3] = 1;
         Accel_Ref[0] = 0;
         Accel_Ref[1] = 0;
         Accel_Ref[2] = 0;
-        Accel_Gain[0] = 0.50;
-        Accel_Gain[1] = 0.50;
-        Accel_Gain[2] = 0.50;
+//        Accel_Gain[0] = 0.50;
+//        Accel_Gain[1] = 0.50;
+//        Accel_Gain[2] = 0.50;
+        Model_Type = 0;
         Gyro_Gain_Setting = -1;
         Cutoff_Freq=0.15;
         Flight_Time=360;
         LCD_Contrast = 60;
         PWM_Mode = 1;
         ESC_Low= 1080;
-        PWM_Interval = 2200; 
-//        Accel_Rang = 2;
-//        Accel_Rate = 14;
-//        PWM_Resolution = 0;
+        PWM_Interval = 5000; 
         StartMode = 'C';
-        Angle_Control = 'H';    // H:horizn  A:angle
-        kp[0] = 1.5;
-        kp[1] = 1.5;
-        kp[2] = 1.5;
-        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;
+        PID_Interval = 0.005;
+        Active_Jyro_Gain = 0.5; 
     }
 };
 #endif
@@ -160,3 +158,14 @@
 
 
 
+
+
+
+
+
+
+
+
+
+
+

--- a/main.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/main.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -1,3 +1,17 @@
+/*  PWM Output
+        type    |   M1      |   M2      |   M3      |   M4      |   M5      |   M6
+    ------------+-----------+-----------+-----------+-----------+---------------------
+    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 |   - 
+    Airplane    |Thr ESC    |Ail Ser    |Ele Ser    |Rud Ser    |Ail Ser    |   -
+    
+    Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Ale:Alevon
+    ESC:for ESC, Ser:for Servo, VP:Variable Pitch
+    F:Front,  B:Back,  L:Light,  R:Right
+*/
+
 #include "mbed.h"
 #include "math.h"
 #include "I2cPeripherals.h"
@@ -6,107 +20,127 @@
 #include "PulseWidthCounter.h"
 #include "string"
 #include "SerialLcd.h"
-#include  "IAP.h"
-#include "PID.h"
+//#include "PID.h"
 #include "SoftPWM.h"
+#include "PulseOut.h"
+#include "Limiter.h"
+#ifndef TARGET_NUCLEO_F401RE
+    #include  "IAP.h"
+#endif
 
 //Serial pc(USBTX, USBRX); 
 
-//Gravity at Earth's surface in m/s/s
-#define g0 9.812865328
-//Convert from radians to degrees.
-#define toDegrees(x) (x * 57.2957795)
-//Convert from degrees to radians.
-#define toRadians(x) (x * 0.01745329252)
-//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
-#define GYROSCOPE_GAIN (1 / 14.375)
-//Full scale resolution on the ADXL345 is 4mg/LSB.
-#define ACCELEROMETER_GAIN (0.004 * g0)
-//Updating filter at 40Hz.
-#define FILTER_RATE 0.05
-//At rest the gyroscope is centred around 0 and goes between about
-//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
-//5/15 = 0.3 degrees/sec.
+#if defined(TARGET_LPC1768)
+    DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
+//    #ifdef LPCXpresso
+//        #define LED1 P0_22
+//    #endif
+    DigitalOut led1(LED1);
+//    DigitalOut led2(LED2);
+    InterruptIn ch1(p5);
+    PulseWidthCounter ch[6] = { p6,p7,p8,p9,p10,p11 };
+    PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 };
+//    SoftPWM pwm[6] = { p21,p22,p23,p24,p25,p26 };
+    SoftPWM buzz(p20);
+    I2cPeripherals i2c(p28,p27); //sda scl
+    SerialLcd lcd(p13,p14);
+    #define MEM_SIZE        256
+    #define TARGET_SECTOR   29     //  use sector 29 as target sector if it is on LPC1768
+    IAP iap;
+#elif defined(TARGET_NUCLEO_F401RE)
+    DigitalOut led1(LED1);
+    InterruptIn ch1(PC_2);
+//    PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 };
+    PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 };
+    PwmOut pwm[6] = { D8,D9,D10,D11,D12,D14 };
+//    PwmOut pwm[6] = { D2,D3,D4,D5,D6,D7 };
+//    SoftPWM pwm[6] = { PB_3,PB_4,PB_10,PC_6,PB_6,PA_7 };
+    SoftPWM buzz(PB_13);
+//    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
+    I2cPeripherals i2c(D5,D7); //sda scl
+    SerialLcd lcd(PA_11,PA_12);
+    #define MEM_SIZE        256
+    #define EXTERNAL_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
+#elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+    DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 };
+    DigitalOut led1(P0_21);
+//    DigitalOut led2(P0_21);
+    InterruptIn ch1(P0_9);
+    PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 };
+//    SoftPWM pwm[4] = { P0_14,P0_2,P0_23,P0_17 };
+    PulseOut pwm[6] = { P0_14,P0_2,P0_23,P0_17,p0_20,p015 };
+    Ticker Tpwm;
+    #define SOFTPWM
+    SoftPWM buzz(P1_19);
+    I2cPeripherals i2c(P0_5,P0_4); //sda scl
+    SerialLcd lcd(P0_19,P0_18);
+    #define MEM_SIZE        256
+    #define TARGET_EEPROM_ADDRESS   64
+//    #define EXTERNAL_EEPROM
+    #define INTERNAL_EEPROM
+    IAP iap;
+#elif defined(TARGET_LPC1114)   // LPC1114
+    DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 };
+    DigitalOut led1(dp28);
+    InterruptIn ch1(dp4);
+    PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 };
+//    SoftPWM pwm[4] = { dp1,dp2,dp18,dp24 };
+    PulseOut pwm[6] = { dp1,dp2,dp18,dp24,dp17,dp6 };
+    Ticker Tpwm;
+    #define SOFTPWM
+    SoftPWM buzz(dp25);
+    I2cPeripherals i2c(dp5,dp27); //sda scl
+    SerialLcd lcd(dp16,dp15);
+    #define MEM_SIZE        256
+    #define EXTERNAL_EEPROM
+#endif
 
-#ifdef TARGET_LPC1114   // LPC1114
-//    PulseOut pwm[4] = { dp1,dp2,dp18,dp24 );
-    #define LED1 dp28
-    #define p5 dp4
-    #define p6 dp9
-    #define p7 dp10
-    #define p8 dp11
-    #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
-    #ifdef LPCXpresso
-        #define LED1 P0_22
-    #endif
-#endif
-DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-InterruptIn ch1(p5);
-PulseWidthCounter ch[5] = { p6,p7,p8,p9,p10 };
-#if defined(SOFT_PWM) || defined(TARGET_LPC1114)
-SoftPWM pwm[4] = { p21,p22,p23,p24 };
-#else
-PwmOut pwm[4] = { p21,p22,p23,p24 };
-#endif
-SoftPWM buzz(p26);
 Timer CurTime;
 //Timer ElapTime;
 Timer CycleTime;
 Timer FlyghtTime;
-//Ticker tick;
-//Ticker tick100ms;
-//Ticker mixTime;
-I2cPeripherals i2c(p28,p27); //sda scl
-SerialLcd lcd(p13);
 config conf;
-PID pid[3];
-#ifdef TARGET_LPC1114
-//LPC1114 Flash Memory read/write
-    #define MEM_SIZE        256
-    #define TARGET_SECTOR   7     //  use sector 29 as target sector if it is on LPC1768
-#else
-//LPC1768 Flash Memory read/write
-    #define MEM_SIZE        256
-    #define TARGET_SECTOR   29     //  use sector 29 as target sector if it is on LPC1768
-#endif
-#ifndef LocalFileOut
-IAP iap;
-#endif
+//PID pid[4];
+Limiter gyroLimit[3] = {300,300,300};
+Limiter accLimit[3] = {0.5,0.5,0.5};
+Limiter pwmLimit[4] = {50,50,50,50};
+//PID height;
 float TotalTime = 0;;
 int channel = 0;
-//int intrupt_cnt = 0;
-//int intrupt_cnt2 = 0;
-int volatile CH[5];
+int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 };
+volatile int CH[9];
 volatile int M[6];
 volatile float Gyro[3];
-volatile float Accel[3];
-volatile float Accel_Save[3];
+volatile float Accel[3]= {0,0,0};
+volatile float Accel_Angle[3];
+volatile float Accel_Save[3]= {0,0,0};
 volatile float Angle[3];
 volatile float Gyro_Ref[3];
 volatile float Gyro_Save[3];
-volatile int Stick[5];
-volatile int Stick_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
+                        1080,1500,1500,1500,1500,1080,      //Delta
+                        1080,1500,1500,1500,1080,1500,      //Delta_TW
+                        1080,1500,1500,1500,1500,1080      //AirPlane
+                        };
 //int Stick_Max[3];
 float Press;
+float Base_Press;
 char InPulseMode;               //Receiver Signal Type    's':Serial, 'P':Parallel
 //volatile bool tick_flag;
 //volatile bool buzz_flag;
-float interval;
+volatile float interval;
 float pid_interval;
-int pid_reg[3];
+//int pid_reg[4];
 int loop_cnt;
+float target_height;
+float cuurent_height;
+float base_throttol;
+int throttol;
+bool hov_control;
+float Rdata;
 
 void initialize();
 void FlashLED(int );
@@ -128,18 +162,19 @@
 void LCD_printf(char *);
 void LCD_cls();
 void LCD_locate(int,int);
-/*
-void tick_interrupt()
+void Servo_Reverse(int,int);
+#ifdef SOFTPWM
+void Tpwm_interrupt()
 {
-    tick_flag = true;    
+    for ( int i=0; i<4; i++ ) pwm[i].start();    
 }
-*/
-void PulseCheck()
+#endif
+void PulseCheck()               //cppm信号のチェック
 {
     channel++;
 }
 
-void PulseAnalysis()            //Interrupt Pin5
+void PulseAnalysis()            //cppm信号の解析
 {
     CurTime.stop();
     int PulseWidth =  CurTime.read_us();
@@ -147,25 +182,9 @@
     CurTime.start();
     if ( PulseWidth > 3000 ) channel = 0;      //reset pulse count
     else {
-        if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
-            switch( channel ) {
-                case IR_THR:
-                    THR = PulseWidth;
-                    break;
-                case IR_AIL:
-                    AIL = PulseWidth;
-                    break;
-                case IR_ELE:
-                    ELE = PulseWidth;
-                    break;
-                case IR_RUD:
-                    RUD = PulseWidth;
-                    break;
-                case IR_AUX:
-                    AUX = PulseWidth;
-                    break;
-            }
-        }
+        if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) {
+            CH[Signal[channel-1]] = PulseWidth;
+      }
     }
     channel++;
 }
@@ -180,8 +199,8 @@
     Get_Stick_Pos();
     while (  Stick[COL] > Thro_Zero || conf.StartMode == 'C' )          //Shrottol Low
     {
-        if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit )              // Shrottle High
-            ESC_SetUp();
+//        if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit )              // Shrottle High
+//            ESC_SetUp();
         if ( Stick[COL] > 890 || conf.StartMode == 'C' )              // Shrottle High
         {
             loop_cnt = 0;
@@ -206,7 +225,10 @@
                     if ( Stick[PIT] < -Stick_Limit )    {       //Elevetor Down
                         loop_cnt = 0;
                         FlashLED(5);
-                        for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
+
+                        for ( int x=0; x<6; x++ ) {
+                            pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]);
+                        }
                         i2c.start(conf.LCD_Contrast);
                         SetUpPrompt(conf,i2c);
                     }                   
@@ -227,9 +249,10 @@
 void  initialize()
 {
     buzz.period_us(400);
+    i2c.start(conf.LCD_Contrast);
+    for ( int i=0;i<6;i++ ) pwm[i].pulsewidth_us(0);
     ReadConfig();               //config.inf file read
 
-    i2c.start(conf.LCD_Contrast);
     channel = 0;
     ch1.rise(&PulseCheck);      //input pulse count
     wait(0.2);
@@ -240,7 +263,10 @@
     else InPulseMode = 'P';
     led1 = 0;
     CycleTime.start();
-    for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+#ifdef SOFTPWM
+    Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
+#endif
+    Base_Press = (float)i2c.pressure() / 4096;
     FlashLED(3);
 }
 
@@ -282,15 +308,40 @@
 #else
     char *send;
     char *recv;
-    int i,rc;
+    int i;
     config *conf_ptr;
     
-    if ( sizeof(config) > 255 ) {
+    if ( sizeof(config) > MEM_SIZE ) {
         LCD_printf("config size over");
         wait(3);
         return;
     }
-    rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
+//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+#if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM)
+    char buf[MEM_SIZE];
+    #if defined(INTERNAL_EEPROM)
+    iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE );
+    #else
+    //External Flash Memory Wreite
+    short pos = 0;
+    if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 )   {
+        while(1)   {
+            FlashLED(3);
+            wait(0.5);
+return;
+        }
+    }
+    #endif
+    send = buf;
+    recv = (char*)&conf;
+    conf_ptr = (config*)buf;
+    if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
+        for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
+        return;
+    }
+    
+#else
+    int rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
     if ( rc == SECTOR_NOT_BLANK ) {
         send = sector_start_adress[TARGET_SECTOR];
         recv = (char*)&conf;
@@ -300,6 +351,7 @@
             return;
         }
     }
+#endif
     WriteConfig();
 #endif
 }
@@ -315,25 +367,27 @@
     char mem[MEM_SIZE];
     char *send;
     int i;
-//pc.printf("start\n\r");
-    if ( sizeof(config) > 255 ) {
+    if ( sizeof(config) > MEM_SIZE ) {
         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;
-//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);
+//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+#if defined(INTERNAL_EEPROM)
+    iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE );
+#elif defined(EXTERNAL_EEPROM)
+//External Flash Memory Wreite
+    short pos = 0;
+    i2c.write_EEPROM( pos,mem,MEM_SIZE) ;
+#else
+    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+    iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+    iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+#endif
 #endif
 }
 
@@ -345,12 +399,14 @@
 //    Stick_Save[ROL] = Stick[ROL];
 //    Stick_Save[PIT] = Stick[PIT];
 //    Stick_Save[YAW] = Stick[YAW];
+//    Stick_Save[COL] = Stick[COL];
     
     Stick[ROL] = AIL - conf.Stick_Ref[ROL];
     Stick[PIT] = ELE - conf.Stick_Ref[PIT];
     Stick[YAW] = RUD - conf.Stick_Ref[YAW];
     Stick[COL] = THR - conf.Stick_Ref[COL];
     Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
+    Stick[AUX2] = AX2 - conf.Stick_Ref[COL];
 }
 
 void Get_Gyro(float interval)
@@ -364,49 +420,37 @@
     if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z);
     else err=i2c.angular(&y,&x,&z);
     if ( err == false ) return;
-    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];       
+    Gyro[ROL] = gyroLimit[0].calc(x) - Gyro_Ref[0] ;
+    Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ;
+    Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ;
+//pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]);
+
 }
 
 bool Get_Accel(float interval)
 {
     float x,y,z;
     bool err;   
-    Accel_Save[ROL] = Accel[ROL];
-    Accel_Save[PIT] = Accel[PIT];
-    Accel_Save[YAW] = Accel[YAW];    
+//    Accel_Save[ROL] = Accel_Angle[ROL];
+//    Accel_Save[PIT] = Accel_Angle[PIT];
+//    Accel_Save[YAW] = Accel_Angle[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;
-//    Accel[ROL] = x - conf.Accel_Ref[0];
-//    Accel[PIT] = y - conf.Accel_Ref[1];
-//    Accel[YAW] = z - conf.Accel_Ref[2];
+//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];
-//    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;  
+    Accel[ROL] = accLimit[0].calc(x);
+    Accel[PIT] = accLimit[1].calc(y);
+    Accel[YAW] = accLimit[2].calc(z);
+//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;
+    Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f;      
+   return true;  
 }
 
 void Get_Angle(float interval)
@@ -415,9 +459,9 @@
     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;
+    x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f;
+    y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f;
+    z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f;
     if ( Get_Accel(interval) == true )  {
         float i =  3.14 * 2 * conf.Cutoff_Freq * interval;
         Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
@@ -428,12 +472,15 @@
         Angle[PIT] += y * interval;
     }
     Angle[YAW] += z * interval;
+//pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]);
 }
 
 void Get_Pressure()
 {
-    Press = i2c.pressure();
-}
+    float P = (float)i2c.pressure()/4096;
+//    Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f );
+    Press = 8.43f * ( P - Base_Press );
+} 
 
 void CalibrateGyros(void)
 {
@@ -471,109 +518,167 @@
     conf.Accel_Ref[0] = k[0]/16;       
     conf.Accel_Ref[1] = k[1]/16;
     conf.Accel_Ref[2] = k[2]/16-1;  
+//    conf.Accel_Ref[2] = k[2]/16;  
 //    FlashLED(3);
 }
 
 void PWM_Out(bool mode)
 {
-   int reg[3];
+    int reg[3];
     int i;
     float gain;
+//    float cur_height;
     
     interval = CycleTime.read();
     CycleTime.reset();
-    if ( interval > 0.2 ) return;
+    if ( interval > 0.2f ) return;
     TotalTime += interval;
-    if ( TotalTime > 0.5 )    {
+    if ( TotalTime > 0.5f )    {
         led1 = !led1; 
-        if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f;
+        if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5;
         else buzz=0.0;
         TotalTime = 0;
     }
     
     Get_Angle(interval);
     pid_interval += interval;
-    if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return; 
+    if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return;
+    pid_interval = 0;
+     
     Get_Stick_Pos();
-    M1 = M2 = M3 = M4 = Stick[COL];
-       
+    switch ( conf.Model_Type )   {
+        case Quad_X:
+            M1 = M2 = M3 = M4 = Stick[COL];
+            break;
+        case Quad_H:
+            M1 = M2 = M3 = M4 = Stick[AUX2];
+            M5 = Stick[COL];
+            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];
+            if ( conf.Model_Type == AirPlane )
+                M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL];
+            else M5 = Stick[COL];
+            break;
+    }
+          
     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 ( fabsf(Stick[i]) > 0 ) gain -= gain * ( fabsf(Stick[i]) / 500 ) * conf.Active_Jyro_Gain; 
         if ( Stick[GAIN] > 0 
-            || i == YAW 
+            || i == YAW
+            || conf.Model_Type > 0
             )  {
             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 ( ( 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    {
-                    if ( (abs(Stick_Save[i]) - abs(Stick[i])) > 10 ) {
-                        Stick_Save[i] = 0;
-                        pid[i].reset();
-                        Angle[i] = 0;
-                    }
-                }
-            } 
-//            else    {
-//                if ( fabsf(Accel[i]) < 0.001 )  {
-//                    pid[i].reset();
-//                    Angle[i] = 0;
-//                }
-//            }
+            reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i];
+            reg[i] += Gyro[i] * gain * GYRO_ADJUST;
         }
-        pid_reg[i] = reg[i];
+        
+//        pid_reg[i] = reg[i];
     }
     pid_interval = 0;
-    //Calculate Roll Pulse Width
-    M1 += reg[ROL];
-    M2 -= reg[ROL];
-    M3 -= reg[ROL];
-    M4 += reg[ROL];
+    
+    switch ( conf.Model_Type )  {
+    case Quad_X:
+    case Quad_H:
+       //Calculate Roll Pulse Width
+        M1 += reg[ROL];
+        M2 -= reg[ROL];
+        M3 -= reg[ROL];
+        M4 += reg[ROL];
 
-    //Calculate Pitch Pulse Width
-    M1 += reg[PIT];
-    M2 += reg[PIT];
-    M3 -= reg[PIT];
-    M4 -= reg[PIT];
+        //Calculate Pitch Pulse Width
+        M1 += reg[PIT];
+        M2 += reg[PIT];
+        M3 -= reg[PIT];
+        M4 -= reg[PIT];
 
-    //Calculate Yaw Pulse Width
-    M1 -= reg[YAW];
-    M2 += reg[YAW];
-    M3 -= reg[YAW];
-    M4 += reg[YAW];
-    
-    for ( i=0;i<4;i++ )
-    {
-//        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;     // motor idle level
+        //Calculate Yaw Pulse Width
+        M1 -= reg[YAW];
+        M2 += reg[YAW];
+        M3 -= reg[YAW];
+        M4 += reg[YAW];
+        break;
+    case Delta:
+    case Delta_TW:    
+        //Calculate Roll Pulse Width
+        M2 += reg[ROL];
+        M3 -= reg[ROL];
+        //Calculate Pitch Pulse Width
+        M2 += reg[PIT];
+        M3 += reg[PIT];
+        //Calculate Yaw Pulse Width
+        M4 -= reg[YAW];
+        //Calculate Yaw Pulse Width
+        if ( conf.Model_Type == Delta_TW ) {
+            M1 += reg[YAW];
+            M5 -= reg[YAW];
+        }
+        break;
+    case AirPlane:
+    //Calculate Roll Pulse Width
+        M2 -= reg[ROL];
+        M5 -= reg[ROL];
+    //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++ )
+        {
+            if ( M[i] > Thro_Hi )   M[i] = Thro_Hi; 
+            if ( M[i] < Thro_Lo )   M[i] = Thro_Lo;     // motor idle level
+        }
     }
     
-    if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
-    
+    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 ) {
-        for ( i=0;i<4;i++ ) {
-           while ( !pwmpin[i] );
-           if ( conf.PWM_Mode == 1 )
+        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]);
+            else pwm[i].pulsewidth_us(M[i]);
         }
     }
         
 }
 
+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)    {
         Get_Stick_Pos();
@@ -585,16 +690,23 @@
 void Flight_SetUp(void)
 {
     int i;
-    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]
-                                            ,conf.PID_Limit,conf.Differential_Limit);
+#ifdef SOFTPWM
+    Tpwm.detach();
+    Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
+#else
+    for ( i=0;i<6;i++ ) pwm[i].pulsewidth_us(0);
+    for ( i=0;i<6;i++ ) pwm[i].period_us(conf.PWM_Interval);
+#endif
+    for ( i=0; i<6; i++ ) {
+        pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
+    }
+    hov_control = false;
     Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
     loop_cnt = 0;
     FlyghtTime.start();
     CycleTime.start();
     pid_interval = 0;
+    Stick_Save[COL] = Stick[COL];
     FlashLED(5);
 }
 
@@ -625,3 +737,9 @@
 
 
 
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Oct 14 08:15:03 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1
\ No newline at end of file

--- a/mbed.lib	Thu Feb 13 16:07:07 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed/#673126e12c73