File content as of revision 0:cca1c4e84da4:

#include "mbed.h"
#include "I2cPeripherals.h"
//#include "I2cLCD.h"
#include "InterruptIn.h"
//#include "ITG3200.h"
//#include "L3GD20.h"
#include "config.h"
#include "PulseWidthCounter.h"
#include "string"
#include "SerialLcd.h"
#include  "IAP.h"
//LPC1768 Flash Memory read/write
#define     MEM_SIZE        256
#define     TARGET_SECTOR    29     //  use sector 29 as target sector if it is on LPC1768
IAP iap;

#ifdef LPCXpresso
DigitalOut led1(P0_22);
#define led2 led1
#else
DigitalOut led1(LED1);
DigitalOut led2(LED2);
#endif
InterruptIn ch1(p5);
PulseWidthCounter ch[5] = { PulseWidthCounter(p6,POSITIVE),
                       PulseWidthCounter(p7,POSITIVE),
                       PulseWidthCounter(p8,POSITIVE),
                       PulseWidthCounter(p9,POSITIVE),
                       PulseWidthCounter(p10,POSITIVE)
                     };
PwmOut pwm[4] = { p21,p22,p23,p24 };
Timer CurTime;
Timer ElapTime;
I2cPeripherals i2c(p28,p27); //sda scl
#ifdef SERIAL_LCD
SerialLcd lcd(p13);
#endif
config conf;
int StartTime;
int Channel = 0;
int CH[5];
int M[6];
int Gyro[3];
int Accel[3];
int Gyro_Ref[3];
int Accel_Ref[3];
int Stick[5];
float Press;
char InPulseMode;               //Receiver Signal Type    's':Serial, 'P':Parallel

void initialize();
void FlashLED(int );
void SetUp();
#ifdef SERIAL_LCD
void SetUpPrompt(config&,SerialLcd&);
#else
void SetUpPrompt(config&,I2cPeripherals&);
#endif
void PWM_Out(bool);
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
void Get_Gyro();
void Get_Accel();
void ReadConfig();
void WriteConfig();
void ESC_SetUp(void);

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,j=0;
    
    initialize();
    wait(0.5);
    Get_Stick_Pos();
    while (  Stick[COL] > 30 || conf.StartMode == 'C' )          //Shrottol Low
    {
        if ( Stick[COL] > 350 || conf.StartMode == 'C' )              // Shrottle High
        {
#ifdef SERIAL_LCD
            SetUpPrompt(conf,lcd);
#else
            SetUpPrompt(conf,i2c);
#endif                          
            for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
            break;
        }
        FlashLED(3);
        wait(1);
        Get_Stick_Pos();
    }
    led2 = 1;
    ElapTime.start();

    while (1)
    {
        if ( Stick[COL] < 30 )
        {
            i = 0;
            ElapTime.stop();
            while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 )
            {
                if ( i > 100 )                          //wait 2 sec
                {
                    CalibrateGyros();
                    CalibrateAccel();
                    FlashLED(6);
                    ElapTime.start();
                    break;
                }
                wait(0.01);                           // wait 10 msec
                Get_Stick_Pos();
                i++;
            }
        }
        j++;
        if (j>100) { j=0; led2 = !led2;}
        ElapTime.stop();
        wait(float(conf.PWM_Interval-ElapTime.read_us()-2)/1000000);
        ElapTime.reset();
        ElapTime.start();
        PWM_Out(true);
    }

}

void  initialize()
{
#ifndef SERIAL_LCD
    i2c.start(ST7032_ADDR,conf.LCD_Contrast);
#endif         
    ReadConfig();               //config.inf file read
//    CurTime.start();
    Channel = 0;
    ch1.rise(&PulseCheck);      //input pulse count
    wait(0.1);
    if ( Channel > 30 )    {
        ch1.rise(&PulseAnalysis);
        InPulseMode = 'S';
    }
    else InPulseMode = 'P';
    if ( conf.Gyro_Type == _ITG3200 )
        i2c.start(ITG3200_ADDR0);
    else
        i2c.start(L3GD20_ADDR1);
    CalibrateGyros();
    i2c.start(LDXL345_ADDR0);
    CalibrateGyros();
    CalibrateAccel();
    i2c.start(LPS331AP_ADDR1);
    for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
}

void FlashLED(int cnt)
{
    for ( int i = 0 ; i < cnt ; i++ ) {
        led1 = !led1;
        wait(0.05);
        led1 = !led1;
        wait(0.05);
    }
}

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 ) {
        i2c.printf("config size over");
        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];
//            i2c.printf("config read OK");
//            wait(1);
            return;
        }
    }
    WriteConfig();
//    i2c.printf("config write OK");
//    wait(1);
#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 ) {
        printf("config size over");
        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 );
    iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
    iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
#endif
}

void Get_Stick_Pos(void)
{
    if ( InPulseMode == 'P' ) {
        for (int i=0;i<5;i++) CH[i] = ch[i].count;
    }
    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()
{
    int x,y,z;
    if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
    else i2c.angular(&y,&x,&z);
    Gyro[ROL] = ( x - Gyro_Ref[0] ) / 5;
    Gyro[PIT] = ( y - Gyro_Ref[1] ) / 5;
    Gyro[YAW] = ( z - Gyro_Ref[2] ) / 5;
}

void Get_Accel()
{
    int x,y,z;
    if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
    else i2c.Acceleration(&y,&x,&z);
    Accel[ROL] = ( x - Accel_Ref[0] );
    Accel[PIT] = ( y - Accel_Ref[1] );
    Accel[YAW] = ( z - Accel_Ref[2] );      
}
void Get_Pressure()
{
    Press = i2c.pressure();
}

void CalibrateGyros(void)
{
    int i,j,x,y,z;
    int k[3]={0,0,0};
    wait(1);
    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.005);
    }
    for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16;
    FlashLED(3);
}

void CalibrateAccel(void)
{
    int i,j,x,y,z;
    int k[3]={0,0,0};
    wait(1);
    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.005);
    }
    for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16;
    FlashLED(3);
}

void PWM_Out(bool mode)
{
   int reg[3];
    int i;
    float gain;
    
//    wait(0.002);
    Get_Stick_Pos();
    Get_Gyro();
//    Get_Accel();

    M1 = M2 = M3 = M4 = Stick[COL];
    for ( i=0;i<3;i++ )
    {
//      Stick Angle Mixing
        if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
        else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
        reg[i] = ( Stick[i] * conf.Stick_Mix[i] ) + ( Gyro[i] * gain );
//        if ( Stick[GAIN] < 0 )
//            reg[i] += Accel[i] * conf.Accel_Gain[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 ( 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] < 20 ) M1=M2=M3=M4=0;
    if ( mode )
        for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+M[i]);

}

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