File content as of revision 8:1db19b529b22:

/*  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    |   -
    Quad-3D     |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC |   -       |   -  
    Delta       |FL Tro ESC |L Alv Ser  |R Alv Ser  |Rud Ser    |   -       |   -     
    Delta-TW    |FL Tro ESC |L Alv Ser  |R Alv Ser  |Rud Ser    |FR Thr ESC |   - 
    Airplane    |Thr ESC    |Ail Ser    |Ele Ser    |Rud Ser    |Ail Ser    |   -
    
    F:Front,  B:Back,  L:Left,  R:Right
    Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon
    ESC:for ESC, Ser:for Servo, VP:Variable Pitch
*/

#include "mbed.h"
#include "math.h"
#include "I2cPeripherals.h"
#include "InterruptIn.h"
#include "config.h"
#include "PulseWidthCounter.h"
#include "string"
#include "SerialLcd.h"

//#if defined(TARGET_NUCLEO_FXXXXX)
//    #include "eeprom_flash.h"
//#endif
//#include "PID.h"
//#include "SoftPWM.h"
#include "Limiter.h"
#include  "IAP.h"

#define DEBUG
//Serial pc(USBTX, USBRX); 
//Serial pc(PA_9,PA_10); 

#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] = { PulseWidthCounter(p6),
                                PulseWidthCounter(p7),
                                PulseWidthCounter(p8),
                                PulseWidthCounter(p9),
                                PulseWidthCounter(p10),
                                PulseWidthCounter(p11) };
    PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 };
//    SoftPWM buzz(p20);
    I2cPeripherals i2c(p28,p27); //sda scl
    SerialLcd lcd(p13,p14);
    #define PwmNum          6
    #define MEM_SIZE        256
    #define TARGET_SECTOR   29     //  use sector 29 as target sector if it is on LPC1768
    IAP iap;
#elif defined(TARGET_STM32F1)
//#define NAZE32BORD
  #ifdef NAZE32BORD
    DigitalOut led1(PC_13);
    InterruptIn ch1(PA_0);
    PulseWidthCounter ch[6] = { PA_8,PA_11,PB_6,PB_7,PB_8,PB_9};
    PwmOut pwm[6] = { PA_0,PA_1,PA_2,PA_3,PA_6,PA_7 };
//    SoftPWM buzz(PA_2);
//    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
    I2cPeripherals i2c(PB_9,PB_8); //sda scl
    SerialLcd lcd(PA_2);
    #define PwmNum          6
    #define MEM_SIZE        256
    #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
  #else
    DigitalOut led1(PC_13);
    InterruptIn ch1(PA_0);
    PulseWidthCounter ch[6] = { PA_11,PA_12,PA_15,PB_3,PB_4,PB_5};
    PwmOut pwm[6] = { PA_6,PA_7,PB_0,PB_1,PB_10,PB_11 };
//    SoftPWM buzz(PA_2);
//    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
    I2cPeripherals i2c(PB_9,PB_8); //sda scl
    SerialLcd lcd(PA_2);
    #define PwmNum          6
    #define MEM_SIZE        256
    #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
  #endif
#elif defined(TARGET_STM32F3)
    DigitalOut led1(PB_3);
    InterruptIn ch1(PA_0);
    PulseWidthCounter ch[6] = { PA_0,PA_1,PB_11,PB_10,PB_4,PB_5};
    PwmOut pwm[6] = { PA_6,PA_7,PA_11,PA_12,PB_8,PB_9 };
//    SoftPWM buzz(PA_2);
//    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
    I2cPeripherals i2c(PB_7,PB_6); //sda scl
    SerialLcd lcd(PA_9);
    #define PwmNum          6
    #define MEM_SIZE        256
    #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
#elif defined(TARGET_NUCLEO_F4)
    DigitalOut led1(PA_5);
    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] = { D3,D5,D6,D9,D11,D12 };
//    SoftPWM buzz(PB_13);
//    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
    I2cPeripherals i2c(PB_9,PB_8); //sda scl
    SerialLcd lcd(PA_11);
    #define PwmNum          6
    #define MEM_SIZE        256
    #define STM32_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 };
    PwmOut pwm[4] = { P0_14,P0_2,P0_23,P0_17 };
//    SoftPWM buzz(P1_19);
    I2cPeripherals i2c(P0_5,P0_4); //sda scl
    SerialLcd lcd(P0_19,P0_18);
    #define PwmNum          4
    #define MEM_SIZE        256
    #define TARGET_EEPROM_ADDRESS   64
    #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 };
    PwmOut pwm[4] = { dp1,dp2,dp18,dp24 };
//    SoftPWM buzz(dp25);
    I2cPeripherals i2c(dp5,dp27); //sda scl
    SerialLcd lcd(dp16,dp15);
    #define PwmNum          4
    #define MEM_SIZE        256
    #define EXTERNAL_EEPROM
#elif defined(TARGET_TEENSY3_1)   // Teensy3.1
    DigitalOut led1(D13);
    InterruptIn ch1(D2);
    PwmOut pwm[6] = { D3,D4,D5,D6,d20,D21 };
    I2cPeripherals i2c(D18,D19); //sda scl
    SerialLcd lcd(D3,D4);   //TX,RX
    #define PwmNum          6
    #define MEM_SIZE        256
    #define EXTERNAL_EEPROM
#endif

Timer CurTime;
//Timer ElapTime;
Timer CycleTime;
//Timer FlyghtTime;
config conf;
//PID pid[4];
//Limiter throLimit = 100;
Limiter gyroLimit[3] = {0.9,0.9,0.9};
Limiter accLimit[3] = {0.95,0.95,0.95};
//Limiter pwmLimit[4] = {50,50,50,50};
//PID height;
float TotalTime = 0;;
int channel = 0;
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]= {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[6];
volatile int Stick_Save[6];
int PWM_Init[6][6] = {  1080,1080,1080,1080,1080,1080,      //Quad_X
                        1500,1500,1500,1500,1080,1080,      //Quad_VP
                        1500,1500,1500,1500,1080,1080,      //Quad_3D
                        1080,1500,1500,1500,1500,1080,      //Delta
                        1080,1500,1500,1500,1080,1500,      //Delta_TW
                        1080,1500,1500,1500,1500,1080      //AirPlane
                        };
char Servo_idx[6][6] = { 0,0,0,0,0,0,      //Quad_X
                         1,1,1,1,0,0,      //Quad_VP
                         0,0,0,0,0,0,      //Quad_3D
                         0,1,1,1,1,1,      //Delta
                         0,1,1,1,0,1,      //Delta_TW
                         0,1,1,1,1,1      //AirPlane
                        };
//int Stick_Max[3];
float Press;
float Base_Press;
char InPulseMode;               //Receiver Signal Type    's':Serial, 'P':Parallel
//volatile bool tick_flag;
//volatile bool buzz_flag;
volatile float interval;
float pid_interval;
//int pid_reg[4];
int loop_cnt;
float target_height;
float cuurent_height;
float base_Throttl;
int Throttl;
bool hov_control;
float Rdata;

void initialize();
void FlashLED(int ,float tm=0.1);
void SetUp();
void SetUpPrompt(config&,I2cPeripherals&);
void PWM_Out(bool);
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
void Get_Gyro(float);
bool Get_Accel(float);
void Get_Angle(float);
void ReadConfig();
void WriteConfig();
void Interrupt_Check(bool&,int &,DigitalOut &);
void ESC_SetUp(void);
void Flight_SetUp();
//void IMUfilter_Reset(void);
void LCD_printf(char *);
void LCD_cls();
void LCD_locate(int,int);
void wait(float);

void PulseCheck()               //cppm信号のチェック
{
    channel++;
}

void PulseAnalysis()            //cppm信号の解析
{
    CurTime.stop();
    int PulseWidth =  CurTime.read_us();
    CurTime.reset();
    CurTime.start();
    if ( PulseWidth > 3000 ) channel = 0;      //reset pulse count
    else {
        if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) {
            CH[Signal[channel-1]] = PulseWidth;
      }
    }
    channel++;
}

int main()
{
    int i=0;
    
    wait(1.5);
    initialize();

    Get_Stick_Pos();
#ifdef DEBUG
            char str[18];
            while ( THR > 1800 ) {
                LCD_locate(0,0);
                sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
                LCD_printf(str);
                LCD_locate(0,1);
                sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
                LCD_printf(str);
                Get_Stick_Pos();
            }
#endif
    while (  Stick[COL] > Thro_Zero || conf.StartMode == 'C'
             || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) )          //Shrottol Low
    {
//        if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit )              // Shrottle High
//            ESC_SetUp();
        if ( Stick[COL] > 890 || conf.StartMode == 'C' )              // Shrottle High
        {
            loop_cnt = 0;
            SetUpPrompt(conf,i2c);
            break;
        }
        FlashLED(2);
        wait(0.5);
        Get_Stick_Pos();
    }
    Flight_SetUp();
    while (1)
    {
        if ( Stick[COL] < Thro_Zero )
        {
            i = 0;
            while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero )      //Rudder Left
            {
                if ( i > 100 )                          //wait 2 sec
                {
//                    FlyghtTime.stop();
                    if ( Stick[PIT] < -Stick_Limit )    {       //Elevetor Down
                        loop_cnt = 0;
                        FlashLED(5);

                        for ( int x=0; x<PwmNum; x++ ) {
                            pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]);
                        }
                        i2c.start(conf.LCD_Contrast);
                        SetUpPrompt(conf,i2c);
                    }                   
                    CalibrateGyros();
                    Flight_SetUp();
                    break;
                }
                wait(0.01);                           // wait 10 msec
                Get_Stick_Pos();
                i++;
            }
        }
        PWM_Out(true);
    }

}

void  initialize()
{
    i2c.start(conf.LCD_Contrast);
    for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
    ReadConfig();               //config.inf file read

    channel = 0;
//#if defined(TARGET_LPC1114)   // LPC1114

    ch1.rise(&PulseCheck);      //input pulse count
    wait(0.2);
    if ( channel > 50 )    {
        FlashLED(50,0.02);
        ch1.rise(&PulseAnalysis);
        InPulseMode = 'S';    }
    else InPulseMode = 'P';
//#endif
    led1 = 0;
    CycleTime.start();
//    throLimit.differential(conf.Thro_Limit_Val);
//    throLimit.rate(conf.Thro_Limit_Rate);
//    Base_Press = (float)i2c.pressure() / 4096;
    FlashLED(10,0.05);
}

void FlashLED(int cnt,float tm)
{
    for ( int i = 0 ; i < cnt ; i++ ) {
        led1 = !led1;
//        buzz = 0.5f;
        wait(tm);
        led1 = !led1;
//        buzz = 0.0f;
        wait(tm);
    }
}

void ReadConfig()
{
#ifdef LocalFileOut
    LocalFileSystem local("local");               // Create the local filesystem under the name "local"
    FILE *fp = fopen("/local/setup.inf", "rb");  // Open "out.txt" on the local file system for writing
    if ( fp != NULL )  {
        float rev = conf.Revision;
        int len = fread(&conf,1,sizeof(config),fp);
        switch ( len ) {
            case  sizeof(config):    // File size ok
                if ( rev == conf.Revision ) break;
            default:
                fclose(fp);
                config init;
                conf = init;
                fp = fopen("/local/setup.inf", "wb");
                fwrite(&conf,1,sizeof(config),fp);
        }
        fclose(fp);
    } else  {
        WriteConfig();
        wait(2);
    }
#else
    char *send;
    char *recv;
    int i;
    char buf[MEM_SIZE];
    config *conf_ptr;
    
    if ( sizeof(config) > MEM_SIZE ) {
//        pc.printf("config size over");
        wait(3);
        return;
    }
//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
#if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM) || defined(STM32_EEPROM)
    #if defined(INTERNAL_EEPROM)
    iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE );
    #elif defined(EXTERNAL_EEPROM) 
    //External Flash Memory Wreite
    short pos = 0;
    if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 )   {
        while(1)   {
            FlashLED(3);
            wait(0.5);
        }
    }
    #else       //STM32 emulate eeprom
//    EEPROM_Read(0,buf,MEM_SIZE)
    readEEPROM(0,(uint32_t*)buf,sizeof(config));
    #endif
    send = buf;
    conf_ptr = (config*)buf;
//pc.printf("rev=%f",conf_ptr->Revision);
    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;
        conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
        if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
            for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
            return;
        }
    }
#endif
    WriteConfig();
#endif
}

void WriteConfig()
{
#ifdef LocalFileOut
    LocalFileSystem local("local");               // Create the local filesystem under the name "local"
    FILE *fp = fopen("/local/setup.inf", "wb");
    fwrite(&conf,1,sizeof(config),fp);
    fclose(fp);
#else
    char mem[MEM_SIZE]= " ";;
    char *send;
    int i;
    if ( sizeof(config) > MEM_SIZE ) {
//        pc.printf("config size over");
        wait(3);
        return;
    }
    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;
//#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) ;
    #elif defined(STM32_EEPROM)
//    EEPROM_Write(0,buf,MEM_SIZE);
        enableEEPROMWriting();
        writeEEPROM(0, (uint32_t *)mem,sizeof(config));
        disableEEPROMWriting();
//    pc.printf("rev=%f,rev=%d",((config*)mem)->Revision,readEEPROMWord(0));
    #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
        for ( i=0; i<4; i++ )   {
            FlashLED(10,0.03);
            wait(0.5);
        }
#endif
}

void Get_Stick_Pos(void)
{
#ifndef TARGET_LPC1114   // LPC1114
    if ( InPulseMode == 'P' ) {
        for (int i=0;i<5;i++) CH[i] = ch[i].count;
    }
#endif
//    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)
{
    float x,y,z;
    bool err;
    Gyro_Save[ROL] = Gyro[ROL];
    Gyro_Save[PIT] = Gyro[PIT];
    Gyro_Save[YAW] = Gyro[YAW];
    
    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] = 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_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;
//pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z);

    x = accLimit[0].calc(x) - conf.Accel_Ref[0];
    y = accLimit[1].calc(y) - conf.Accel_Ref[1];
    z = accLimit[2].calc(z) - conf.Accel_Ref[2];
//pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]);
    Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f;
    Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f;
    Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f;      
   return true;  
}

void Get_Angle(float interval)
{
    float x,y,z;
//    Get_Accel(interval);
    Get_Gyro(interval);

    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;
        Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
    }
    else    {  
        Angle[ROL] += x * interval;
        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()
{
    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)
{
    int i;
    float x,y,z;
    float k[3]={0,0,0};
    wait(1);
    Angle[0]=Angle[1]=Angle[2]=0;
    for(i=0; i<16; i++) {
        if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
        else i2c.angular(&y,&x,&z);
        k[0] += x;
        k[1] += y;
        k[2] += z;
        wait(0.01);
    }
    for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16;
//    FlashLED(3);
}

void CalibrateAccel(void)
{
    int i;
    float x,y,z;
    float k[3]={0,0,0};
    conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0;
    for(i=0; i<16; i++) {
        if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
        else i2c.Acceleration(&y,&x,&z);
        k[0] += x;
        k[1] += y;
        k[2] += z;
        wait(0.01);
    }
    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 i,j,k;
    float gain;
//    float cur_height;
    
    interval = CycleTime.read();
    CycleTime.reset();
    if ( interval > 0.005f && mode ) return;
    TotalTime += interval;
    if ( TotalTime > 0.5f )    {
        led1 = !led1; 
 //       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 < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return;
    pid_interval = 0;
     
    Get_Stick_Pos();
    switch ( conf.Model_Type )   {
        case Quad_X:
            M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim;
            break;
        case Quad_VP:
            M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL];
            M5 = THR + conf.Throttl_Trim;
            break;
        case Quad_3D:
            j = THR + conf.Throttl_Trim;
            if ( Stick[GAIN] < 0 )  {
                k = THR - conf.Reverse_Point;
                if ( abs(k) < THR_MIDRANGE )   { 
                    if ( k > 0 ) j = conf.Reverse_Point + THR_MIDRANGE;
                    else j = conf.Reverse_Point - THR_MIDRANGE;
                }
            }
//           i = (int)throLimit.calc((float)THR);
 //           if ( abs(i-conf.Reverse_Point) > 100 ) i = THR;
            M1 = M2 = M3 = M4 = j;
            break;
        case Delta:
        case Delta_TW:
        case AirPlane:
            M1 = THR + conf.Throttl_Trim;
            M2 = conf.Stick_Ref[ROL];
            M3 = conf.Stick_Ref[PIT];
            M4 = conf.Stick_Ref[YAW];
            if ( conf.Model_Type == AirPlane )
                M5 = conf.Stick_Ref[ROL];
            else M5 = M1;
            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)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
        if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain;
        if ( Stick[GAIN] > 0 
            || i == YAW
            || conf.Model_Type > 0
            || i2c.GetAddr(ACCEL_ADDR) == 0
            )  {
            reg[i] = Stick[i] * conf.Stick_Mix[i];
            reg[i] += Gyro[i] * gain * GYRO_ADJUST;
        }
        else   {
            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_interval = 0;
    
//    int Reverse = 1;
    switch ( conf.Model_Type )  {
      case Quad_3D:
//        if ( THR < conf.Reverse_Point )  { 
//            Reverse = -1;
//        }
      case Quad_X:
      case Quad_VP:
       //Calculate Roll Pulse Width
        M1 += reg[ROL];
        M2 -= reg[ROL];
        M3 -= reg[ROL];
        M4 += reg[ROL];

        //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];
        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;
    }
    j = conf.Model_Type;
    for ( i=0; i<PwmNum; i++ ) {
        if ( M[i] > Pulse_Max )   M[i] = Pulse_Max; 
        if ( M[i] < Pulse_Min )   M[i] = Pulse_Min;
        if ( Servo_idx[j][i] == 1 )
        {
            M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] );
        }
        else    {
            if ( j == Quad_3D ) {
                if ( Stick[GAIN] > 0 )  {
                    if ( THR < conf.Reverse_Point ) 
                        M[i] = conf.Reverse_Point;
                }
            }
            else    {
                if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL];
            }
        }
    }
    if ( mode ) {
//        h = conf.Stick_Ref[THR];
        for ( i=0;i<PwmNum;i++ ) {
//           while ( !pwmpin[i] );
            if ( conf.PWM_Mode == 1 )
                pwm[i].pulsewidth_us(M[i]);
            else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]);
        }
    }
        
}

        
void ESC_SetUp(void)    {
    while(1)    {
        Get_Stick_Pos();
        for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
        wait(0.015);
    }
}

void Flight_SetUp(void)
{
    int i;
    for ( i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
    for ( i=0;i<PwmNum;i++ ) pwm[i].period_us(conf.PWM_Interval);
    for ( i=0; i<PwmNum; i++ ) {
        pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
    }
    hov_control = false;
//    throLimit.differential(conf.Thro_Limit_Val);
//    throLimit.rate(conf.Thro_Limit_Rate);
    Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
    loop_cnt = 0;
//    FlyghtTime.start();
    CycleTime.start();
    pid_interval = 0;
    Stick_Save[COL] = Stick[COL];
    FlashLED(5);
}

void LCD_locate(int clm,int row)
{
    lcd.locate(clm,row);
    i2c.locate(clm,row);
}

void LCD_cls()
{
    lcd.cls();
    i2c.cls();
}

void LCD_printf(char* str)
{
    lcd.write(str);
    i2c.write_lcd(str);
}

void wait(float tm)
{
    wait_us(tm*1000000);
}
;