Diff: config.cpp

Revision:

4:4060309b9cc0

Parent:

3:27407c4984cf

Child:

6:a50e6d3924f1

--- a/config.cpp	Thu Feb 13 16:07:07 2014 +0000
+++ b/config.cpp	Tue Oct 14 08:15:03 2014 +0000
@@ -4,22 +4,27 @@
 #include "config.h"
 #include "PulseWidthCounter.h"
 #include "SerialLcd.h"
-#include "PID.h"
+//#include "PID.h"
 
 //Serial pc(USBTX, USBRX); 
 
-#define CALIBURATE 10
-#define GYROGAIN 20
-#define GYRODIR 30
-#define ACCELCORRECT 40
-#define PIDSET 50
-#define STICKMIX 60
-#define DISPPULSE 70
-#define DISPSENSOR 80
-#define DISPPWM 90 
-#define PARMSET 100
-#define CONFSTORE 110
-#define CONFRESET 120
+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);
@@ -42,23 +47,27 @@
 
 Timer elaps;
 
-extern volatile int CH[5];
+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[5];
+extern volatile int Stick[6];
 extern volatile float Press;
 extern volatile float interval;
 //extern bool tick_flag;
-extern PID pid[3];
-extern int pid_reg[3];
+//extern PID pid[4];
+//extern PID height;
+//extern int pid_reg[4];
 const char steering[3][6]= {"Roll ","Pitch","Yaw  "};
-short mode;
+const char ModelName[5][9] = { "Quad-X  ","Quad-H  ","Delta   ","Delta-TW","AirPlane" };    
+int mode;//
 char sw,ret_mode;
-short vnum,hnum,vmax,hmax;
-short idx,i;
+int vnum,hnum,vmax,hmax;//
+int idx,i;//
 char str[33];
 config init;
 
@@ -69,7 +78,7 @@
     mode = 0;
     vnum = 0;
     hnum = 0;
-    vmax = 12;
+    vmax = FINAL - 1;
 
     while( 1 ) {
 //        FlashLED(1);
@@ -77,20 +86,26 @@
         mode = vnum * 10 + hnum;
 
         switch ( mode ) {
+            
+            //初期画面
             case 0:
                 LCD_locate(0,0);
-                sprintf(str,"Quad-X  Ver %4.2f",conf.Revision);
+                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 = 0;
                 break;
-            case CALIBURATE:        //Calibrate Transmitter
+
+            //送信機信号のキャリブレーション
+            case CALIBURATE*10:        //Calibrate Transmitter
                 LCD_printf("Calibrate");
                 Set_Arrow(1);
                 hmax = 1;
                 break;
-            case CALIBURATE+1:        //Calibrate Transmitter
+            case CALIBURATE*10+1:        //Calibrate Transmitter
                 LCD_printf("Start Calibrate");
                 wait(1);
                 for(i=0; i<4; i++)  {
@@ -108,6 +123,7 @@
                 for(i=0; i<4; i++) {
                     conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
                 }
+
                 CalibrateGyros();
                 CalibrateAccel();
                 LCD_cls();                 //Clear LCD
@@ -116,30 +132,32 @@
                 FlashLED(5);
                 hnum = 0;
                 break;
-            case GYROGAIN:        //Set Gyro Gain
+
+            //ジャイロ感度の設定
+            case GYROGAIN*10:        //Set Gyro Gain
                 LCD_printf("Set Gyro Gain");
                 Set_Arrow(1);
                 hmax = 4;
                 break;
-            case GYROGAIN+1:                                //Set Gyro Gain Roll
+            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+2:
+            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+3:
+            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+4:
+            case GYROGAIN*10+4:
 //                ret_mode = 'R';
                 LCD_printf("GyroGain>setting");
                 LCD_locate(0,1);
@@ -154,18 +172,20 @@
                     LCD_printf("Transmitter");
                 Set_Arrow(3);
                 break;
-            case GYRODIR:        //Set Gyro Direction
+                
+            //ジャイロの効きの逆転                
+            case GYRODIR*10:        //Set Gyro Direction
                 LCD_printf("Gyro Direction");
                 Set_Arrow(1);
                 hmax = 4;
                 break;
-            case GYRODIR+1:                                //Set Gyro Direction Roll
-            case GYRODIR+2:
-            case GYRODIR+3:
-            case GYRODIR+4:
+            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+1);
-                if ( mode == (GYRODIR+4) )
+                idx = mode - (GYRODIR*10+1);
+                if ( mode == (GYRODIR*10+4) )
                     LCD_printf("Gyro>Swap X-Y");
                 else {
                     LCD_printf("Gyro>Dir>");
@@ -182,82 +202,86 @@
                     LCD_printf("Normal ");
                 else
                     LCD_printf("Reverse");
-                if ( mode == (GYRODIR+4) )
+                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:
+
+            //加速度計の水平レベルの校正
+            case ACCELCORRECT*10:
                 LCD_printf("Acceleration");
                 LCD_locate(2,1);
-                LCD_printf("correction");
+                LCD_printf("Trim");
                 Set_Arrow(1);
                 hmax = 3;
                 break;
-            case ACCELCORRECT+1:
+            case ACCELCORRECT*10+1:
                 Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw);
                 break;
-            case ACCELCORRECT+2:
+            case ACCELCORRECT*10+2:
                 Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw);
                 break;
-            case ACCELCORRECT+3:
+            case ACCELCORRECT*10+3:
                 Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.001f,sw);
                 break;
-            case PIDSET:        //PID Setting
-                LCD_printf("PID Setting");
-                Set_Arrow(1);
-                hmax = 9;
-                break;
-            case PIDSET+1:
-                Param_Set_Prompt1("PID>RoolPitch>kp",&conf.kp[0],2,0.00f,15.00f,0.01f,sw);
-                conf.kp[1] = conf.kp[0];
-                break;
-            case PIDSET+2:                               
-                Param_Set_Prompt1("PID>RoolPitch>ki",&conf.ki[0],2,0.00f,5.00f,0.01f,sw);
-                conf.ki[1] = conf.ki[0];
-                break;
-            case PIDSET+3:                                
-                Param_Set_Prompt1("PID>RoolPitch>kd",&conf.kd[0],2,0.00f,5.00f,0.01f,sw);
-                conf.kd[1] = conf.kd[0];
-                break;
-            case PIDSET+4:
-                Param_Set_Prompt1("PID>YAW>kp",&conf.kp[2],2,0.00f,15.00f,0.01f,sw);
-                break;
-            case PIDSET+5:                               
-                Param_Set_Prompt1("PID>YAW>ki",&conf.ki[2],2,0.00f,5.00f,0.01f,sw);
-                break;
-            case PIDSET+6:                                
-                Param_Set_Prompt1("PID>YAW>kd",&conf.kd[2],2,0.00f,5.00f,0.01f,sw);
-                break;
-            case PIDSET+7:                                
-                Param_Set_Prompt1("PID Interval",&conf.PID_Interval,2,0.002f,0.015f,0.001f,sw);
-                break;
-            case PIDSET+8:                                
-                Param_Set_Prompt1("PID Limit",&conf.PID_Limit,2,0.00f,400.00f,10.00f,sw);
-                break;
-            case PIDSET+9:                                
-                Param_Set_Prompt1("differential Lim",&conf.Differential_Limit,3,0.00f,300.00f,10.00f,sw);
-                break;
-            case STICKMIX:        //Set Stick Mixing
+
+            //スティック操作量の設定
+            case STICKMIX*10:        //Set Stick Mixing
                 LCD_printf("Set Stick Mixing");
                 Set_Arrow(1);
                 hmax = 3;
                 break;
-            case STICKMIX+1:                                //Set Stick Mixing
+            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+2:
+            case STICKMIX*10+2:
                 Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
                 break;
-            case STICKMIX+3:
+            case STICKMIX*10+3:
                 Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
                 break;
-            case DISPPULSE:        //Display Pulse Width
+
+            //送信機パルス長の表示
+            case DISPPULSE*10:        //Display Pulse Width
                 LCD_printf("Disp Pulse Width");
                 Set_Arrow(1);
-                hmax = 2;
+                hmax = 3;
+                x = conf.ESC_Low;
                 break;
-            case DISPPULSE+1:        //Display Pulse Width
+            case DISPPULSE*10+1:        //Display Pulse Width
 //           DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
                 ret_mode = 'R';
                 LCD_locate(0,0);
@@ -267,7 +291,17 @@
                 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
                 LCD_printf(str);
                 break;
-            case DISPPULSE+2:        //Display Stick Ref
+            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);
@@ -277,17 +311,16 @@
                 sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
                 LCD_printf(str);
                 break;
-            case DISPSENSOR:        //Display Sensor Value
-                LCD_printf("Disp Senser");
+
+            //センサー値の表示
+            case DISPSENSOR*10:        //Display Sensor Value
+                LCD_printf("Disp Sensor");
                 Set_Arrow(1);
-                hmax = 7;
-                for ( i=0; i<3; i++ )   {
-                    pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])/50.0
-                            ,conf.PID_Limit,conf.Differential_Limit);
-                    Angle[i] = 0;
-                }
+                hmax = 6;
+                Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
+                Accel[ROL]=Accel[PIT]=Accel[YAW]=0;
                 break;
-            case DISPSENSOR+1:        //Gyro
+            case DISPSENSOR*10+1:        //Gyro
 //                Get_Gyro();
                 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
                 else i2c.angular(&y,&x,&z);
@@ -302,15 +335,14 @@
                 LCD_printf(str);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+2:            //Accelerometer
-//                Get_Accel();
+            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,"[Accel]X=%5.2f",x);
+                sprintf(str,"[Gravity]X=%5.2f",x);
                 LCD_printf(str);
                 LCD_locate(0,1);
                 sprintf(str,"Y=%5.2f,Z=%5.2f",y,z);
@@ -318,7 +350,7 @@
 //                Set_Arrow(2);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+3:            //angle
+            case DISPSENSOR*10+3:            //angle
                 PWM_Out(false);
                 LCD_locate(0,0);
                 sprintf(str,"[Angle]X=%6.1f",Angle[ROL]);
@@ -329,33 +361,22 @@
 //                Set_Arrow(2);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+4:
-                PWM_Out(false);
-                LCD_locate(0,0);
-                sprintf(str,"t=%5d,a=%6.2f",-Stick[YAW]*45/400,Angle[YAW]);
-                LCD_printf(str);
-                LCD_locate(0,1);
-                sprintf(str,"pid=%5d",pid_reg[YAW]);
-                LCD_printf(str);
-                Set_Arrow(2);
-                ret_mode = 'R';
-                break;
-            case DISPSENSOR+5:                // Pressure
+            case DISPSENSOR*10+4:                // Pressure
                 elaps.reset();
                 elaps.start();
                 Get_Pressure();
                 elaps.stop();
                 LCD_locate(0,0);
-                sprintf(str,"Press=%9.1f",Press/4096);
+                sprintf(str,"Press=%4.1fhp",Press);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"Elaps=%6d",elaps.read_us());
+                sprintf(str,"Height=%7.2fcm",i2c.height_cm());
                 LCD_printf(str);
 //                Set_Arrow(2);
                 ret_mode = 'R';
                 wait(0.05);
                 break;
-            case DISPSENSOR+6:
+            case DISPSENSOR*10+5:
                 elaps.reset();
                 elaps.start();
                 PWM_Out(false);
@@ -364,39 +385,53 @@
                 LCD_locate(0,0);
                 sprintf(str,"ElapsTime=%6d",i);
                 LCD_printf(str);
-                Set_Arrow(2);
+//                Set_Arrow(2);
                 ret_mode = 'R';
                 break;
-            case DISPSENSOR+7:        //Sensor Calibration
+            case DISPSENSOR*10+6:        //Sensor Calibration
                 CalibrateGyros();
                 FlashLED(3);
                 LCD_printf("Calibrate completed");
                 Set_Arrow(3);
                 break;
-            case DISPPWM:                                //Display PWM Condition
+
+
+            //ＥＳＣ用PWMパルス長の表示
+            case DISPPWM*10:                                //Display PWM Condition
                 LCD_printf("Display PWM ");
                 Set_Arrow(1);
-                hmax = 1;
+                hmax = 2;
                 break;
-            case DISPPWM+1:                                //Display PWM Width
+            case DISPPWM*10+1:                                //Display PWM Width
                 ret_mode = 'R';
                 PWM_Out(false);
+                i = conf.ESC_Low;
                 LCD_locate(0,0);
-                sprintf(str,"M1=%4d,M2=%4d",M1,M2);
+                sprintf(str,"M1=%4d,M2=%4d",i+M1,i+M2);
                 LCD_printf(str);
                 LCD_locate(0,1);
-                sprintf(str,"M4=%4d,M3=%4d",M4,M3);
+                sprintf(str,"M4=%4d,M3=%4d",i+M4,i+M3);
                 LCD_printf(str);
                 break;
-            case PARMSET:    //パラメーター設定
+            case DISPPWM*10+2:                                //Display PWM Width
+                ret_mode = 'R';
+                PWM_Out(false);
+                i = conf.ESC_Low;
+                LCD_locate(0,0);
+                sprintf(str,"M5=%4d,M6=%4d",i+M5,i+M6);
+                LCD_printf(str);
+                break;
+
+            //その他パラメータ値の設定
+            case PARMSET*10:    //パラメーター設定
                 LCD_printf("Parameter Set");
                 Set_Arrow(1);
-                hmax = 6;
+                hmax = 8;
                 break;
-            case PARMSET+1:
+            case PARMSET*10+1:
                 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
                 break;
-            case PARMSET+2:
+            case PARMSET*10+2:
                 LCD_locate(0,0);
                 LCD_printf("PWM>Mode");
                 LCD_locate(0,1);
@@ -411,27 +446,45 @@
                     LCD_printf("Moter");
                 Set_Arrow(2);
                 break;
-            case PARMSET+3:
-                Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
+            case PARMSET*10+3:
+                Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,20000,100,sw);
                 break;
-            case PARMSET+4:                                
+            case PARMSET*10+4:                                
                 Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
                 break;
-            case PARMSET+5:
+            case PARMSET*10+5:
                 Param_Set_Prompt1("ESC>Low Position",&conf.ESC_Low,2,Pulse_Min,Pulse_Max,1,sw);
                 break;
-            case PARMSET+6:
+            case PARMSET*10+6:
                 Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
                 break;
-//            case PARMSET+7:
-//                Param_Set_Prompt1("Cont. swap Angle",&conf.Control_Exchange_Angle,3,0.0f,40.0f,1.0f,sw);
-//                break;
-            case CONFSTORE:       //E2PROM Store
+            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_Jyro_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+1:
+            case CONFSTORE*10+1:
                 WriteConfig();
                 LCD_locate(0,0);
                 sprintf(str,"Config %3dbyte",sizeof(config));
@@ -443,18 +496,20 @@
                 FlashLED(5);
                 hnum = 0;
                 break;
-            case CONFRESET:       //E2PROM reset
+
+            //設定データの初期化
+            case CONFRESET*10:       //E2PROM reset
                 LCD_printf("Config Reset");
                 Set_Arrow(1);
                 hmax = 3;
                 break;
-            case CONFRESET+1:
+            case CONFRESET*10+1:
                 LCD_printf("Ailron stick");
                 LCD_locate(0,1);
                 LCD_printf("Move to right");
                 Set_Arrow(2);
                 break;
-            case CONFRESET+2:       //E2PROM reset
+            case CONFRESET*10+2:       //E2PROM reset
                 conf = init;
                 LCD_printf("Rset sucssesuful");
                 Set_Arrow(3);
@@ -620,52 +675,4 @@
         case 3:
             LCD_printf("  <<");
     }
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+};