File content as of revision 6:a50e6d3924f1:

#include "mbed.h"
#include "I2cPeripherals.h"
#include "InterruptIn.h"
#include "config.h"
#include "PulseWidthCounter.h"
#include "SerialLcd.h"
#include "Limiter.h"
//#include "PID.h"

//Serial pc(USBTX, USBRX); 

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);
void Param_Set_Prompt1(char *,int *,int,int,int,int,char);
void Param_Set_Prompt1(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 Get_Angle(float);
void PWM_Out(bool);
void WriteConfig();
void ESC_SetUp(void);
void Get_Pressure();
void LCD_printf(char *);
void LCD_cls();
void LCD_locate(int,int);

Timer elaps;

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[6];
extern volatile float Press;
extern volatile float interval;
extern Limiter throLimit;
//extern bool tick_flag;
//extern PID pid[4];
//extern PID height;
//extern int pid_reg[4];
const char steering[3][6]= {"Roll ","Pitch","Yaw  "};
const char ModelName[6][9] = { "Quad-X  ","Quad-VP ","Quad-3D ","Delta   ","Delta-TW","AirPlane" };    
int mode;//
char sw,ret_mode;
int vnum,hnum,vmax,hmax;//
int idx,i;//
char str[33];
config init;

void SetUpPrompt(config& conf,I2cPeripherals& i2c)
{
    float x,y,z;
    LCD_cls();
    mode = 0;
    vnum = 0;
    hnum = 0;
    vmax = FINAL - 1;

    while( 1 ) {
//        FlashLED(1);
        ret_mode = 'W';
        mode = vnum * 10 + hnum;

        switch ( mode ) {
            
            //初期画面
            case 0:
                LCD_locate(0,0);
                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 = 1;
                break;
            case 1:         //モデルタイプの設定
                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 = 4;
                        break;
                    case 'U':
                        if ( conf.Model_Type < 5 ) conf.Model_Type += 1;
                        else conf.Model_Type = 0;
                }
                LCD_printf( (char*)ModelName[conf.Model_Type] );
                Set_Arrow(2);
                break;

            //送信機信号のキャリブレーション
            case CALIBURATE*10:        //Calibrate Transmitter
                LCD_printf("Calibrate");
                Set_Arrow(1);
                hmax = 1;
                break;
            case CALIBURATE*10+1:        //Calibrate Transmitter
                LCD_printf("Start Calibrate");
                wait(1);
                for(i=0; i<4; i++)  {
                    conf.Stick_Ref[i] = 0;
                }
                for(i=0; i<16; i++) {
                    wait(0.03);
                    Get_Stick_Pos();                   
                    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;
                }

                CalibrateGyros();
                CalibrateAccel();
                LCD_cls();                 //Clear LCD
                LCD_printf("Calibrate Completed");
                Set_Arrow(3);
                FlashLED(5);
                hnum = 0;
                break;

            //ジャイロ感度の設定
            case GYROGAIN*10:        //Set Gyro Gain
                LCD_printf("Set Gyro Gain");
                Set_Arrow(1);
                hmax = 5;
                break;
            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*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*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*10+4:
                Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Gyro_Gain,3,0.0f,1.0f,0.01f,sw);
                break;
            case GYROGAIN*10+5:
//                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 GYRODIR*10:        //Set Gyro Direction
                LCD_printf("Gyro Direction");
                Set_Arrow(1);
                hmax = 4;
                break;
            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*10+1);
                if ( mode == (GYRODIR*10+4) )
                    LCD_printf("Gyro>Swap X-Y");
                else {
                    LCD_printf("Gyro>Dir>");
                    LCD_locate(9,0);
                    LCD_printf((char*)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 == (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*10:
                LCD_printf("Acceleration");
                LCD_locate(2,1);
                LCD_printf("Trim");
                Set_Arrow(1);
                hmax = 3;
                break;
            case ACCELCORRECT*10+1:
                Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw);
                break;
            case ACCELCORRECT*10+2:
                Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw);
                break;
            case ACCELCORRECT*10+3:
                Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.001f,sw);
                break;

            //スティック操作量の設定
            case STICKMIX*10:        //Set Stick Mixing
                LCD_printf("Set Stick Mixing");
                Set_Arrow(1);
                hmax = 3;
                break;
            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*10+2:
                Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
                break;
            case STICKMIX*10+3:
                Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
                break;

            //送信機パルス長の表示
            case DISPPULSE*10:        //Display Pulse Width
                LCD_printf("Disp Pulse Width");
                Set_Arrow(1);
                hmax = 3;
                break;
            case DISPPULSE*10+1:        //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 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);
                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 DISPSENSOR*10:        //Display Sensor Value
                LCD_printf("Disp Sensor");
                Set_Arrow(1);
                hmax = 6;
                Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
                Accel[ROL]=Accel[PIT]=Accel[YAW]=0;
                break;
            case DISPSENSOR*10+1:        //Gyro
//                Get_Gyro();
                if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
                else i2c.angular(&y,&x,&z);
                x -= Gyro_Ref[0];
                y -= Gyro_Ref[1];
                z -= Gyro_Ref[2];
                LCD_locate(0,0);
                sprintf(str,"[Gyro]X=%5.1f",x);
                LCD_printf(str);
                LCD_locate(0,1);
                sprintf(str,"y=%5.1f,Z=%5.1f",y,z);
                LCD_printf(str);
                ret_mode = 'R';
                break;
            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,"[Gravity]X=%5.2f",x);
                LCD_printf(str);
                LCD_locate(0,1);
                sprintf(str,"Y=%5.2f,Z=%5.2f",y,z);
                LCD_printf(str);
//                Set_Arrow(2);
                ret_mode = 'R';
                break;
            case DISPSENSOR*10+3:            //angle
                PWM_Out(false);
                LCD_locate(0,0);
                sprintf(str,"[Angle]X=%6.1f",Angle[ROL]);
                LCD_printf(str);
                LCD_locate(0,1);
                sprintf(str,"Y=%6.1f,Z=%5.1f",Angle[PIT],Angle[YAW]);
                LCD_printf(str);
//                Set_Arrow(2);
                ret_mode = 'R';
                break;
            case DISPSENSOR*10+4:                // Pressure
                elaps.reset();
                elaps.start();
                Get_Pressure();
                elaps.stop();
                LCD_locate(0,0);
                sprintf(str,"Press=%4.1fhp",Press);
                LCD_printf(str);
                LCD_locate(0,1);
                sprintf(str,"Height=%7.2fcm",i2c.height_cm());
                LCD_printf(str);
//                Set_Arrow(2);
                ret_mode = 'R';
                wait(0.05);
                break;
            case DISPSENSOR*10+5:
                elaps.reset();
                elaps.start();
                PWM_Out(false);
                elaps.stop();
                i = elaps.read_us();
                LCD_locate(0,0);
                sprintf(str,"ElapsTime=%6d",i);
                LCD_printf(str);
//                Set_Arrow(2);
                ret_mode = 'R';
                break;
            case DISPSENSOR*10+6:        //Sensor Calibration
                CalibrateGyros();
                FlashLED(3);
                LCD_printf("Calibrate completed");
                Set_Arrow(3);
                break;


            //ＥＳＣ用PWMパルス長の表示
            case DISPPWM*10:                                //Display PWM Condition
                LCD_printf("Display PWM ");
                Set_Arrow(1);
                hmax = 2;
                break;
            case DISPPWM*10+1:                                //Display PWM Width
                ret_mode = 'R';
                PWM_Out(false);
                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 DISPPWM*10+2:                                //Display PWM Width
                ret_mode = 'R';
                PWM_Out(false);
                LCD_locate(0,0);
                sprintf(str,"M5=%4d,M6=%4d",M5,M6);
                LCD_printf(str);
                break;

            //その他パラメータ値の設定
            case PARMSET*10:    //パラメーター設定
                LCD_printf("Parameter Set");
                Set_Arrow(1);
                hmax = 9;
                break;
            case PARMSET*10+1:
                Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
                break;
            case PARMSET*10+2:
                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 PARMSET*10+3:
                Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,20000,100,sw);
                break;
            case PARMSET*10+4:                                
                Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
                break;
            case PARMSET*10+5:
                Param_Set_Prompt1("ESC>Throttl Trim",&conf.Throttl_Trim,2,Pulse_Min,Pulse_Max,1,sw);
                break;
            case PARMSET*10+6:
                Param_Set_Prompt1("ESC>ReversePoint",&conf.Reverse_Point,2,1000,2000,1,sw);
                break;
            case PARMSET*10+7:
                Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
                break;
            case PARMSET*10+8:
                Param_Set_Prompt1("Thro Limit val",&conf.Thro_Limit_Val,2,0,200,1,sw);
                throLimit.differential(conf.Thro_Limit_Val);
                break;
            case PARMSET*10+9:
                Param_Set_Prompt1("Thro Limit Rate",&conf.Thro_Limit_Rate,3,0,1,0.01,sw);
                throLimit.rate(conf.Thro_Limit_Rate);
                break;
/*            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_Gyro_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*10+1:
                WriteConfig();
                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(0.5);
                FlashLED(5);
                hnum = 0;
                break;

            //設定データの初期化
            case CONFRESET*10:       //E2PROM reset
                LCD_printf("Config Reset");
                Set_Arrow(1);
                hmax = 3;
                break;
            case CONFRESET*10+1:
                LCD_printf("Ailron stick");
                LCD_locate(0,1);
                LCD_printf("Move to right");
                Set_Arrow(2);
                break;
            case CONFRESET*10+2:       //E2PROM reset
                conf = init;
                LCD_printf("Rset sucssesuful");
                Set_Arrow(3);
                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':
                while ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) {
                    FlashLED(2);
                    wait(0.5);
                }
                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();
        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(char *hd,int *num,int arrow,int min,int max,int increase,char sw)
{
    ret_mode = 'R';
    LCD_locate(0,0);
    LCD_printf(hd);
    LCD_locate(0,1);
    sprintf(str,"%6d",*num);
    LCD_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 Param_Set_Prompt1(char *hd,float *num,int arrow,float min,float max,float increase,char sw)
{
    ret_mode = 'R';
    LCD_locate(0,0);
    LCD_printf(hd);
    LCD_locate(0,1);
    sprintf(str,"%7.3f",*num);
    LCD_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)
{
    LCD_locate(12,1);
    switch ( dir ) {
        case 1:
            LCD_printf("  >>");
            break;
        case 2:
            LCD_printf("<<>>");
            break;
        case 3:
            LCD_printf("  <<");
    }
};