File content as of revision 2:59ac9df97701:

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

//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.

#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 dp25
    #define p10 dp26
    #define p13 dp16
    #define p21 dp1
    #define p22 dp2
    #define p23 dp18
    #define p24 dp24
    #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
float TotalTime = 0;;
int channel = 0;
//int intrupt_cnt = 0;
//int intrupt_cnt2 = 0;
volatile int CH[5];
volatile int M[6];
volatile float Gyro[3];
volatile float Accel[3];
volatile float Angle[3];
volatile float Gyro_Ref[3];
volatile float Gyro_Save[3];
volatile int Stick[5];
volatile int Stick_Save[3];
//int Stick_Max[3];
float Press;
char InPulseMode;               //Receiver Signal Type    's':Serial, 'P':Parallel
//volatile bool tick_flag;
//volatile bool buzz_flag;
float interval;
int pid_reg[3];
int loop_cnt;

void initialize();
void FlashLED(int );
void SetUp();
void SetUpPrompt(config&,I2cPeripherals&);
void PWM_Out(bool);
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
void Get_Gyro();
bool Get_Accel();
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 tick_interrupt()
{
    tick_flag = true;    
}
*/
void PulseCheck()
{
    channel++;
}

void PulseAnalysis()            //Interrupt Pin5
{
    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 ) {
            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;
            }
        }
    }
    channel++;
}

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

    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 || conf.StartMode == 'C' )              // Shrottle High
        {
            loop_cnt = 0;
            SetUpPrompt(conf,i2c);
            break;
        }
        FlashLED(3);
        wait(0.3);
        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 ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
                        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()
{
    buzz.period_us(500);
    ReadConfig();               //config.inf file read

    i2c.start(conf.LCD_Contrast);
    channel = 0;
    ch1.rise(&PulseCheck);      //input pulse count
    wait(0.2);
    if ( channel > 50 )    {
        ch1.rise(&PulseAnalysis);
        InPulseMode = 'S';
    }
    else InPulseMode = 'P';
    led1 = 0;
    CycleTime.start();
    for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
    FlashLED(3);
}

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

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,rc;
    config *conf_ptr;
    
    if ( sizeof(config) > 255 ) {
        LCD_printf("config size over");
        wait(3);
        return;
    }
    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;
        }
    }
    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) > 255 ) {
        LCD_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;
    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
    iap.erase( TARGET_SECTOR, TARGET_SECTOR );
//pc.printf("erase\n");
    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
    iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
//pc.printf("write\n");
#endif
}

void Get_Stick_Pos(void)
{
    if ( InPulseMode == 'P' ) {
        for (int i=0;i<5;i++) CH[i] = ch[i].count;
    }
//    Stick_Save[ROL] = Stick[ROL];
//    Stick_Save[PIT] = Stick[PIT];
//    Stick_Save[YAW] = Stick[YAW];
    
    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;
}

void Get_Gyro()
{
    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] = x - Gyro_Ref[0] ;
    Gyro[PIT] = y - Gyro_Ref[1] ;
    Gyro[YAW] = z - Gyro_Ref[2] ;
    
    if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL];
    if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT];
    if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW];       
}

bool Get_Accel()
{
    float x,y,z;
    bool err;
    if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
    else err=i2c.Acceleration(&y,&x,&z);
    if ( err == false ) return false;
    float wx = x - conf.Accel_Ref[0];
    float wy = y - conf.Accel_Ref[1];
    float wz = z - conf.Accel_Ref[2];
    Accel[ROL] = atan(wx/sqrt( pow(wy,2)+pow(wz,2)))*180/3.14;
    Accel[PIT] = atan(wy/sqrt( pow(wx,2)+pow(wz,2)))*180/3.14;
    Accel[YAW] = atan(sqrt( pow(wx,2)+pow(wy,2))/wz)*180/3.14;      
    return true;  
}

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

    x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5;
    y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5;
    z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5;
    if ( Get_Accel() == true )  {
        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;
}

void Get_Pressure()
{
    Press = i2c.pressure();
}

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-9.8;
//    FlashLED(3);
}

void PWM_Out(bool mode)
{
   int reg[3];
    int i;
    float gain;
    
    Get_Stick_Pos();
    M1 = M2 = M3 = M4 = Stick[COL];
    interval = CycleTime.read();
    CycleTime.reset();
    if ( interval > 0.1 ) return;
    TotalTime += interval;
    if ( TotalTime > 0.5 )    {
        led1 = !led1; 
        if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f;
        else buzz=0.0;
        TotalTime = 0;
    }
    
    Get_Angle(interval);
    
    for ( i=0;i<3;i++ ) {
            
//      Stick Angle Mixing
        if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
        else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
        if ( Stick[GAIN] > 0 
//            || i == YAW 
//            || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle 
            )  {
            reg[i] = Stick[i] * conf.Stick_Mix[i];
            reg[i] += Gyro[i] * gain * GYRO_ADJUST;
        }
        else   {
            if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) )   {
                if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) )   {
                    Stick_Save[i] = Stick[i]>>2;
                }
                else    {
//                    pc.printf("PID RESET.%3d",i);wait(1);
                    Stick_Save[i] = 0;
                    pid[i].reset();
                    Angle[i] = 0;
                }
            } 
            reg[i] = Stick[i] * conf.Stick_Mix[i];
            reg[i] += pid[i].calc(Angle[i],0,interval);
//            reg[i] = pid[i].calc(Angle[i],-(float)Stick[i]*60/400,interval);
        }
        pid_reg[i] = reg[i];
    }
    //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];
    
    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;     // this is the motor idle level
    }
    
    if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
    
    if ( mode ) {
        for ( i=0;i<4;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]);
        }
    }
        
}

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

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]*(float)abs(Stick[GAIN])*0.02
                                            ,conf.PID_Limit,conf.Integral_Limit);
    Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
    loop_cnt = 0;
    FlyghtTime.start();
    CycleTime.start();
    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.printf(str);
    i2c.printf(str);
}
;