File content as of revision 0:cca1c4e84da4:

#include "mbed.h"
#include "I2cPeripherals.h"
//#include "I2cLCD.h"
#include "InterruptIn.h"
//#include "ITG3200.h"
//#include "LPS331AP.h"
#include "config.h"
#include "PulseWidthCounter.h"
#include "SerialLcd.h"

void FlashLED(int);
char Check_Stick_Dir(char);
void Param_Set_Prompt1(const char *,int *,int,int,int,int,char);
void Param_Set_Prompt1(const char *,float *,int,float,float,float,char);
void Set_Arrow(int dir);
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
void Get_Gyro();
void Get_Accel();
void PWM_Out(bool);
void WriteConfig();
void ESC_SetUp(void);
void Get_Pressure();

Timer elaps;

extern int CH[5];
extern int M[6];
extern int Gyro[3];
extern int Accel[3];
extern int Gyro_Ref[3];
extern int Stick[5];
extern float Press;
const char steering[3][6]= {"Roll ","Pitch","Yaw  "};
short mode;
char sw,ret_mode;
short vnum,hnum,vmax,hmax;
short idx,i;
char str[33];

#ifdef SERIAL_LCD
SerialLcd *lcdptr;
void SetUpPrompt(config& conf,SerialLcd& lcd)
#else
I2cPeripherals *lcdptr;
void SetUpPrompt(config& conf,I2cPeripherals& lcd)
#endif
{

    lcdptr = &lcd;
    lcd.cls();
    mode = 0;
    vnum = 0;
    hnum = 0;
    vmax = 11;
    
    while( 1 ) {
        FlashLED(1);
        ret_mode = 'W';
        mode = vnum * 10 + hnum;
        
        switch ( mode ) {
            case 0:
                lcd.locate(0,0);
                sprintf(str,"Quad-X  Ver %4.2f",conf.Revision);
                lcd.printf(str);
                lcd.locate(4,1);
                lcd.printf("By AZUKITEN");
                hmax = 0;
                break;
            case 10:        //Calibrate Transmitter
                lcd.printf("Calibrate Transmitter");
                Set_Arrow(1);
                hmax = 1;
                break;
            case 11:        //Calibrate Transmitter
                lcd.printf("Start Calibrate");
                for(i=0; i<4; i++)  {
                    conf.Stick_Ref[i] = 0;
                }
                for(i=0; i<16; i++) {
                    wait(0.3);
                    conf.Stick_Ref[ROL] += AIL;
                    conf.Stick_Ref[PIT] += ELE;
                    conf.Stick_Ref[YAW] += RUD;
                    conf.Stick_Ref[COL] += THR;
//                    conf.Stick_Ref[GAIN] += AUX;
                }
                for(i=0; i<4; i++) {
                    conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
                }
                lcd.cls();                 //Clear LCD
                lcd.printf("Calibrate Completed");
                Set_Arrow(3);
                FlashLED(5);
                break;
            case 20:        //Set Gyro Gain
                lcd.printf("Set Gyro Gain");
                Set_Arrow(1);
                hmax = 4;
                break;
            case 21:                                //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 22:
                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 23:
                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 24:
//                ret_mode = 'R';
                lcd.printf("GyroGain>setting");
                lcd.locate(0,1);
                switch ( sw ) {
                    case 'U':
                    case 'D':
                        conf.Gyro_Gain_Setting *= -1;
                }
                if ( conf.Gyro_Gain_Setting == 1 )
                    lcd.printf("Controller");
                else
                    lcd.printf("Transmitter");
                Set_Arrow(3);
                break;
            case 30:        //Set Gyro Direction
                lcd.printf("Gyro Direction");
                Set_Arrow(1);
                hmax = 4;
                break;
            case 31:                                //Set Gyro Direction Roll
            case 32:
            case 33:
            case 34:
 //              ret_mode = 'R';
                idx = mode - 31;
                if ( mode == 34 )
                    lcd.printf("Gyro>Swap X-Y");
                else {
                    lcd.printf("Gyro>Dir>");
                    lcd.locate(9,0);
                    lcd.printf(steering[idx]);
                }
                lcd.locate(0,1);
                switch ( sw ) {
                    case 'U':
                    case 'D':
                        conf.Gyro_Dir[idx] *= -1;
                }
                if ( conf.Gyro_Dir[idx] == 1 )
                    lcd.printf("Normal ");
                else
                    lcd.printf("Reverse");
                if ( mode == 34 )
                    Set_Arrow(3);
                else
                    Set_Arrow(2);
                break;
            case 40:        //Set Accelerometer
                lcd.printf("Acceleration");
                lcd.locate(5,1);
                lcd.printf("Gain");
                Set_Arrow(1);
                hmax = 3;
                break;
            case 41:    
                Param_Set_Prompt1("Accel>Roll",&conf.Accel_Gain[0],2,0.00f,1.00f,0.01f,sw);
                break;
            case 42:                                //Set Accelerometer
                Param_Set_Prompt1("Accel>Pith",&conf.Accel_Gain[1],2,0.00f,1.00f,0.01f,sw);
                break;
            case 43:                                //Set Accelerometer
                ret_mode = 'R';
                Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Gain[2],3,0.00f,1.00f,0.01f,sw);
               break;
                
            case 50:        //Set Stick Mixing
                lcd.printf("Set Stick Mixing");
                Set_Arrow(1);
                hmax = 3;
                break;
            case 51:                                //Set Stick Mixing
                Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
                break;
            case 52:
                Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
                break;
            case 53:
                Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
                break;
            case 60:        //Display Pulse Width
                lcd.printf("Disp Pulse Width");
                Set_Arrow(1);
                hmax = 2;
                break;
            case 61:        //Display Pulse Width
//           DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
               ret_mode = 'R';
                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);
                break;
            case 62:        //Display Stick Ref
//            DisplayPulseWidth(Stick[COL],Stick[ROL],Stick[PIT],Stick[YAW],Stick[GAIN]);
               ret_mode = 'R';
                Get_Stick_Pos();
                lcd.locate(0,0);
                sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
                lcd.printf(str);
                lcd.locate(0,1);
                sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
                lcd.printf(str);
                break;
            case 70:        //Display Sensor Value
                lcd.printf("Disp Senser");
                Set_Arrow(1);
                hmax = 5;
                break;
            case 71:        //Gyro
                Get_Gyro();
                lcd.locate(0,0);
                sprintf(str,"Gyro//X=%5d",Gyro[ROL]);
                lcd.printf(str);
                wait(0.02);
                lcd.locate(0,1);
                sprintf(str,"y=%5d,Z=%5d",Gyro[PIT],Gyro[YAW]);
                lcd.printf(str);
                wait(0.02);
                ret_mode = 'R';
                break;
            case 72:            //Accelerometer
                Get_Accel();
                lcd.locate(0,0);
                sprintf(str,"Accel//X=%5d",Accel[ROL]);
                lcd.printf(str);
                lcd.locate(0,1);
                sprintf(str,"Y=%5d,Z=%5d",Accel[PIT],Accel[YAW]);
                lcd.printf(str);
//                Set_Arrow(2);
                ret_mode = 'R';
                break;
            case 73:                // Pressure           
                elaps.reset();
                elaps.start();
                Get_Pressure();
                elaps.stop();
                lcd.locate(0,0);
                sprintf(str,"Pressure=%9.3f",Press/4096);
                lcd.printf(str);
                lcd.locate(0,1);
                sprintf(str,"Elaps=%6d",elaps.read_us());
                lcd.printf(str);
//                Set_Arrow(2);
                ret_mode = 'R';
                break;
            case 74:
                elaps.reset();
                elaps.start();
                PWM_Out(false);
                elaps.stop();
                lcd.locate(0,0);
                sprintf(str,"ElapsTime=%5d",elaps.read_us());
                lcd.printf(str);
                Set_Arrow(2);
                ret_mode = 'R';
                break;
            case 75:        //Sensor Calibration
//          CalibrateAccel();
                CalibrateGyros();
                CalibrateAccel();
                lcd.printf("Calibrate completed");
                Set_Arrow(3);
                break;
            case 80:                                //Display PMW Condition
                lcd.printf("Display PMW ");
                Set_Arrow(1);
                hmax = 1;
                break;
            case 81:                                //Display PMW Width
                PWM_Out(false);
//            DisplayInt(Motor_HD[0],M1,M2,M4,M3);
               ret_mode = 'R';
                lcd.locate(0,0);
                sprintf(str,"M1=%4d,M2=%4d",M1,M2);
                lcd.printf(str);
                lcd.locate(0,1);
                sprintf(str,"M4=%4d,M3=%4d",M4,M3);
                lcd.printf(str);
                break;
            case 90:    //パラメーター設定
                lcd.printf("Parameter Set");
                Set_Arrow(1);
                hmax = 4;
                break;
            case 91:
                Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
                break;
            case 92:
                lcd.locate(0,0);
                lcd.printf("PWM>Mode");
                lcd.locate(0,1);
                switch ( sw ) {
                    case 'U':
                    case 'D':
                        conf.PWM_Mode *= -1;
                }
                if ( conf.PWM_Mode == 1 )
                    lcd.printf("ESC  ");
                else
                    lcd.printf("Moter");
                Set_Arrow(2);
                break;
            case 93:
                Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
//                for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
                break;
            case 94:
                lcd.locate(0,0);
                lcd.printf("Gyro>Type");
                lcd.locate(0,1);
                switch ( sw ) {
                    case 'U':
                    case 'D':
                        if ( conf.Gyro_Type == _L3GD20 )
                            conf.Gyro_Type = _ITG3200;
                        else
                            conf.Gyro_Type = _L3GD20;
                }
                if ( conf.Gyro_Type == _L3GD20 )    
                    lcd.printf("L3GD20");
                else    
                    lcd.printf("ITG3200");
                Set_Arrow(2);
                break;
            case 100:       //E2PROM Store
                lcd.printf("Config Save");
                Set_Arrow(1);
                hmax = 1;
                break;
            case 101:
                WriteConfig();
                FlashLED(5);
                lcd.locate(0,0);
                sprintf(str,"Config %3dbyte",sizeof(config));
                lcd.printf(str);
                lcd.locate(0,1);
                lcd.printf("Save Sucssesuful");
                Set_Arrow(3);
                wait(2);
                break;
            case 110:                               //ESC Set up
                lcd.locate(0,0);
                lcd.printf("ESC Set UP");
                Set_Arrow(1);
                hmax = 1;
                break;
            case 111:
                if ( conf.PWM_Mode == -1 ) {
                    hnum = 0;
                    break;
                }
                lcd.locate(0,0);
                lcd.printf("Setup Start");
                wait(1.5);
                lcd.cls();                 //Clear LCD
                lcd.locate(0,0);
                lcd.printf("Please power-off");
                lcd.locate(1,0);
                lcd.printf(" after Setup");
                
                ESC_SetUp();
                break;

            default:
                if ( hnum == 0 )
                    vnum++;
        }


        sw = Check_Stick_Dir(ret_mode);             //Wait Mode

        switch ( sw ) {
            case 'L':
                hnum--;
                if ( hnum <= 0 ) hnum = 0;
                lcd.cls();                 //Clear LCD
                break;
            case 'R':
                lcd.cls();
                if ( hnum < hmax ) hnum++;
                break;
            case 'U':
                if ( hnum == 0 ) {
                    if ( vnum < vmax ) vnum++;
                    else vnum = 0;
                    lcd.cls();                 //Clear LCD
                }
                break;
            case 'D':
                if ( hnum == 0 ) {
                    if ( vnum > 0 ) vnum--;
                    else vnum = vmax;
                    lcd.cls();                 //Clear LCD
                }
                break;
            case 'E':
                lcd.cls();                 //Clear LCD
                lcd.locate(0,0);
                lcd.printf("PWM Started");
                return;
        }
    }

}

char Check_Stick_Dir(char act)
{
    int i;
    while ( 1 ) {
        Get_Stick_Pos();
//  DisplayInt(Stick[0],Stick[1],Stick[2],Stick[3]);
//  wait(0.2);
        if ( Stick[YAW] > Stick_Limit ) {
            i = 0;
            while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
                if ( i > 2000 ) return 'E';     //wait 2 sec
                wait(0.001);                            // wait 1 msec
                Get_Stick_Pos();
                i++;
            }
        }
        if ( Stick[ROL] > Stick_Limit ) {
            wait(0.03);
            Get_Stick_Pos();
            if ( !(Stick[ROL] > Stick_Limit) ) continue;
            while ( Stick[ROL] > Stick_Limit ) {
                Get_Stick_Pos();
            }
            return 'R';
        }
        if ( Stick[ROL] < -Stick_Limit ) {
            wait(0.03);
            Get_Stick_Pos();
            if ( !(Stick[ROL] < -Stick_Limit) ) continue;
            while ( Stick[ROL] < -Stick_Limit ) {
                Get_Stick_Pos();
            }
            return 'L';
        }
        if ( Stick[PIT] < -Stick_Limit ) {
            wait(0.03);
            Get_Stick_Pos();
            if ( !(Stick[PIT] < -Stick_Limit) ) continue;
            if ( act == 'R' ) {
                wait(0.03);
                return 'D';
            }
            while ( Stick[PIT] < -Stick_Limit ) {
                Get_Stick_Pos();
            }
            return 'D';
        }
        if ( Stick[PIT] > Stick_Limit ) {
            wait(0.03);
            Get_Stick_Pos();
            if ( !( Stick[PIT] > Stick_Limit) ) continue;
            if ( act == 'R' ) {
                wait(0.03);
                return 'U';
            }
            while ( Stick[PIT] > Stick_Limit ) {
                Get_Stick_Pos();
            }
            return 'U';
        }
        if ( act == 'R' )
            return ' ';
    }
}

void Param_Set_Prompt1(const char *hd,int *num,int arrow,int min,int max,int increase,char sw)
{
    ret_mode = 'R';
    lcdptr->locate(0,0);
    lcdptr->printf(hd);
    lcdptr->locate(0,1);
    lcdptr->printf("%d",*num);
    Set_Arrow(arrow);
    switch ( sw ) {
        case 'U':
            *num -= increase;
            if ( *num <= min )
                *num = min;
            break;
        case 'D':
            *num += increase;
            if ( *num >= max )
                *num = max;
    }
}
void Param_Set_Prompt1(const char *hd,float *num,int arrow,float min,float max,float increase,char sw)
{
    ret_mode = 'R';
    lcdptr->locate(0,0);
    lcdptr->printf(hd);
    lcdptr->locate(0,1);
    sprintf(str,"%7.2f",*num);
    lcdptr->printf(str);
    Set_Arrow(arrow);
    switch ( sw ) {
        case 'U':
            *num -= increase;
            if ( *num <= min )
                *num = min;
            break;
        case 'D':
            *num += increase;
            if ( *num >= max )
                *num = max;
    }
}

void Set_Arrow(int dir)
{
    lcdptr->locate(12,1);
    switch ( dir ) {
        case 1:
            lcdptr->printf("  >>");
            break;
        case 2:
            lcdptr->printf("<<>>");
            break;
        case 3:
            lcdptr->printf("  <<");
    }
}