syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
config.cpp
- Committer:
- komaida424
- Date:
- 2021-02-21
- Revision:
- 8:1db19b529b22
- Parent:
- 6:a50e6d3924f1
File content as of revision 8:1db19b529b22:
#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,float tm=0.1); 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); void wait(float); 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: //xy軸の入れ替え // 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; //ESC用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 = 8; 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("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(" <<"); } };