syouichi imamori
/
MulticopterQuadX
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: config.cpp
- Revision:
- 0:cca1c4e84da4
- Child:
- 2:59ac9df97701
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/config.cpp Thu Jul 11 19:18:44 2013 +0000
@@ -0,0 +1,555 @@
+#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(" <<");
+ }
+}
+
+
+
+
+
+