syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

config.cpp@2:59ac9df97701, 2013-11-15 (annotated)

Committer:
komaida424
Date:
Fri Nov 15 20:53:36 2013 +0000
Revision:
2:59ac9df97701
Parent:
0:cca1c4e84da4
Child:
3:27407c4984cf
ver.1.25

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 0:cca1c4e84da4 1 #include "mbed.h"
komaida424 0:cca1c4e84da4 2 #include "I2cPeripherals.h"
komaida424 0:cca1c4e84da4 3 #include "InterruptIn.h"
komaida424 0:cca1c4e84da4 4 #include "config.h"
komaida424 0:cca1c4e84da4 5 #include "PulseWidthCounter.h"
komaida424 0:cca1c4e84da4 6 #include "SerialLcd.h"
komaida424 2:59ac9df97701 7 #include "PID.h"
komaida424 2:59ac9df97701 8
komaida424 2:59ac9df97701 9 //Serial pc(USBTX, USBRX);
komaida424 2:59ac9df97701 10
komaida424 2:59ac9df97701 11 #define CALIBURATE 10
komaida424 2:59ac9df97701 12 #define GYROGAIN 20
komaida424 2:59ac9df97701 13 #define GYRODIR 30
komaida424 2:59ac9df97701 14 #define ACCELCORRECT 40
komaida424 2:59ac9df97701 15 #define PIDSET 50
komaida424 2:59ac9df97701 16 #define STICKMIX 60
komaida424 2:59ac9df97701 17 #define DISPPULSE 70
komaida424 2:59ac9df97701 18 #define DISPSENSOR 80
komaida424 2:59ac9df97701 19 #define DISPPWM 90
komaida424 2:59ac9df97701 20 #define PARMSET 100
komaida424 2:59ac9df97701 21 #define CONFSTORE 110
komaida424 2:59ac9df97701 22 #define CONFRESET 120
komaida424 0:cca1c4e84da4 23
komaida424 0:cca1c4e84da4 24 void FlashLED(int);
komaida424 0:cca1c4e84da4 25 char Check_Stick_Dir(char);
komaida424 2:59ac9df97701 26 void Param_Set_Prompt1(char *,int *,int,int,int,int,char);
komaida424 2:59ac9df97701 27 void Param_Set_Prompt1(char *,float *,int,float,float,float,char);
komaida424 0:cca1c4e84da4 28 void Set_Arrow(int dir);
komaida424 0:cca1c4e84da4 29 void Get_Stick_Pos();
komaida424 0:cca1c4e84da4 30 void CalibrateGyros(void);
komaida424 0:cca1c4e84da4 31 void CalibrateAccel(void);
komaida424 0:cca1c4e84da4 32 void Get_Gyro();
komaida424 0:cca1c4e84da4 33 void Get_Accel();
komaida424 2:59ac9df97701 34 void Get_Angle(float);
komaida424 0:cca1c4e84da4 35 void PWM_Out(bool);
komaida424 0:cca1c4e84da4 36 void WriteConfig();
komaida424 0:cca1c4e84da4 37 void ESC_SetUp(void);
komaida424 0:cca1c4e84da4 38 void Get_Pressure();
komaida424 2:59ac9df97701 39 void LCD_printf(char *);
komaida424 2:59ac9df97701 40 void LCD_cls();
komaida424 2:59ac9df97701 41 void LCD_locate(int,int);
komaida424 0:cca1c4e84da4 42
komaida424 0:cca1c4e84da4 43 Timer elaps;
komaida424 0:cca1c4e84da4 44
komaida424 2:59ac9df97701 45 extern volatile int CH[5];
komaida424 2:59ac9df97701 46 extern volatile int M[6];
komaida424 2:59ac9df97701 47 extern volatile float Gyro[3];
komaida424 2:59ac9df97701 48 extern volatile float Accel[3];
komaida424 2:59ac9df97701 49 extern volatile float Angle[3];
komaida424 2:59ac9df97701 50 extern volatile float Gyro_Ref[3];
komaida424 2:59ac9df97701 51 extern volatile int Stick[5];
komaida424 2:59ac9df97701 52 extern volatile float Press;
komaida424 2:59ac9df97701 53 extern volatile float interval;
komaida424 2:59ac9df97701 54 //extern bool tick_flag;
komaida424 2:59ac9df97701 55 extern PID pid[3];
komaida424 2:59ac9df97701 56 extern int pid_reg[3];
komaida424 0:cca1c4e84da4 57 const char steering[3][6]= {"Roll ","Pitch","Yaw "};
komaida424 0:cca1c4e84da4 58 short mode;
komaida424 0:cca1c4e84da4 59 char sw,ret_mode;
komaida424 0:cca1c4e84da4 60 short vnum,hnum,vmax,hmax;
komaida424 0:cca1c4e84da4 61 short idx,i;
komaida424 0:cca1c4e84da4 62 char str[33];
komaida424 2:59ac9df97701 63 config init;
komaida424 0:cca1c4e84da4 64
komaida424 2:59ac9df97701 65 void SetUpPrompt(config& conf,I2cPeripherals& i2c)
komaida424 0:cca1c4e84da4 66 {
komaida424 2:59ac9df97701 67 float x,y,z;
komaida424 2:59ac9df97701 68 LCD_cls();
komaida424 0:cca1c4e84da4 69 mode = 0;
komaida424 0:cca1c4e84da4 70 vnum = 0;
komaida424 0:cca1c4e84da4 71 hnum = 0;
komaida424 2:59ac9df97701 72 vmax = 12;
komaida424 2:59ac9df97701 73
komaida424 0:cca1c4e84da4 74 while( 1 ) {
komaida424 2:59ac9df97701 75 // FlashLED(1);
komaida424 0:cca1c4e84da4 76 ret_mode = 'W';
komaida424 0:cca1c4e84da4 77 mode = vnum * 10 + hnum;
komaida424 2:59ac9df97701 78
komaida424 0:cca1c4e84da4 79 switch ( mode ) {
komaida424 0:cca1c4e84da4 80 case 0:
komaida424 2:59ac9df97701 81 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 82 sprintf(str,"Quad-X Ver %4.2f",conf.Revision);
komaida424 2:59ac9df97701 83 LCD_printf(str);
komaida424 2:59ac9df97701 84 LCD_locate(4,1);
komaida424 2:59ac9df97701 85 LCD_printf("By AZUKITEN");
komaida424 0:cca1c4e84da4 86 hmax = 0;
komaida424 0:cca1c4e84da4 87 break;
komaida424 2:59ac9df97701 88 case CALIBURATE: //Calibrate Transmitter
komaida424 2:59ac9df97701 89 LCD_printf("Calibrate");
komaida424 0:cca1c4e84da4 90 Set_Arrow(1);
komaida424 0:cca1c4e84da4 91 hmax = 1;
komaida424 0:cca1c4e84da4 92 break;
komaida424 2:59ac9df97701 93 case CALIBURATE+1: //Calibrate Transmitter
komaida424 2:59ac9df97701 94 LCD_printf("Start Calibrate");
komaida424 0:cca1c4e84da4 95 for(i=0; i<4; i++) {
komaida424 0:cca1c4e84da4 96 conf.Stick_Ref[i] = 0;
komaida424 0:cca1c4e84da4 97 }
komaida424 0:cca1c4e84da4 98 for(i=0; i<16; i++) {
komaida424 0:cca1c4e84da4 99 wait(0.3);
komaida424 0:cca1c4e84da4 100 conf.Stick_Ref[ROL] += AIL;
komaida424 0:cca1c4e84da4 101 conf.Stick_Ref[PIT] += ELE;
komaida424 0:cca1c4e84da4 102 conf.Stick_Ref[YAW] += RUD;
komaida424 0:cca1c4e84da4 103 conf.Stick_Ref[COL] += THR;
komaida424 0:cca1c4e84da4 104 // conf.Stick_Ref[GAIN] += AUX;
komaida424 0:cca1c4e84da4 105 }
komaida424 0:cca1c4e84da4 106 for(i=0; i<4; i++) {
komaida424 0:cca1c4e84da4 107 conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
komaida424 0:cca1c4e84da4 108 }
komaida424 2:59ac9df97701 109 CalibrateGyros();
komaida424 2:59ac9df97701 110 CalibrateAccel();
komaida424 2:59ac9df97701 111 LCD_cls(); //Clear LCD
komaida424 2:59ac9df97701 112 LCD_printf("Calibrate Completed");
komaida424 0:cca1c4e84da4 113 Set_Arrow(3);
komaida424 0:cca1c4e84da4 114 FlashLED(5);
komaida424 2:59ac9df97701 115 hnum = 0;
komaida424 0:cca1c4e84da4 116 break;
komaida424 2:59ac9df97701 117 case GYROGAIN: //Set Gyro Gain
komaida424 2:59ac9df97701 118 LCD_printf("Set Gyro Gain");
komaida424 0:cca1c4e84da4 119 Set_Arrow(1);
komaida424 0:cca1c4e84da4 120 hmax = 4;
komaida424 0:cca1c4e84da4 121 break;
komaida424 2:59ac9df97701 122 case GYROGAIN+1: //Set Gyro Gain Roll
komaida424 0:cca1c4e84da4 123 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 124 Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 125 else
komaida424 2:59ac9df97701 126 Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 127 break;
komaida424 2:59ac9df97701 128 case GYROGAIN+2:
komaida424 0:cca1c4e84da4 129 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 130 Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 131 else
komaida424 0:cca1c4e84da4 132 Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 133 break;
komaida424 2:59ac9df97701 134 case GYROGAIN+3:
komaida424 0:cca1c4e84da4 135 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 0:cca1c4e84da4 136 Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 137 else
komaida424 0:cca1c4e84da4 138 Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 139 break;
komaida424 2:59ac9df97701 140 case GYROGAIN+4:
komaida424 0:cca1c4e84da4 141 // ret_mode = 'R';
komaida424 2:59ac9df97701 142 LCD_printf("GyroGain>setting");
komaida424 2:59ac9df97701 143 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 144 switch ( sw ) {
komaida424 0:cca1c4e84da4 145 case 'U':
komaida424 0:cca1c4e84da4 146 case 'D':
komaida424 0:cca1c4e84da4 147 conf.Gyro_Gain_Setting *= -1;
komaida424 0:cca1c4e84da4 148 }
komaida424 0:cca1c4e84da4 149 if ( conf.Gyro_Gain_Setting == 1 )
komaida424 2:59ac9df97701 150 LCD_printf("Controller");
komaida424 0:cca1c4e84da4 151 else
komaida424 2:59ac9df97701 152 LCD_printf("Transmitter");
komaida424 0:cca1c4e84da4 153 Set_Arrow(3);
komaida424 0:cca1c4e84da4 154 break;
komaida424 2:59ac9df97701 155 case GYRODIR: //Set Gyro Direction
komaida424 2:59ac9df97701 156 LCD_printf("Gyro Direction");
komaida424 0:cca1c4e84da4 157 Set_Arrow(1);
komaida424 0:cca1c4e84da4 158 hmax = 4;
komaida424 0:cca1c4e84da4 159 break;
komaida424 2:59ac9df97701 160 case GYRODIR+1: //Set Gyro Direction Roll
komaida424 2:59ac9df97701 161 case GYRODIR+2:
komaida424 2:59ac9df97701 162 case GYRODIR+3:
komaida424 2:59ac9df97701 163 case GYRODIR+4:
komaida424 2:59ac9df97701 164 // ret_mode = 'R';
komaida424 2:59ac9df97701 165 idx = mode - (GYRODIR+1);
komaida424 2:59ac9df97701 166 if ( mode == (GYRODIR+4) )
komaida424 2:59ac9df97701 167 LCD_printf("Gyro>Swap X-Y");
komaida424 0:cca1c4e84da4 168 else {
komaida424 2:59ac9df97701 169 LCD_printf("Gyro>Dir>");
komaida424 2:59ac9df97701 170 LCD_locate(9,0);
komaida424 2:59ac9df97701 171 LCD_printf((char*)steering[idx]);
komaida424 0:cca1c4e84da4 172 }
komaida424 2:59ac9df97701 173 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 174 switch ( sw ) {
komaida424 0:cca1c4e84da4 175 case 'U':
komaida424 0:cca1c4e84da4 176 case 'D':
komaida424 0:cca1c4e84da4 177 conf.Gyro_Dir[idx] *= -1;
komaida424 0:cca1c4e84da4 178 }
komaida424 0:cca1c4e84da4 179 if ( conf.Gyro_Dir[idx] == 1 )
komaida424 2:59ac9df97701 180 LCD_printf("Normal ");
komaida424 0:cca1c4e84da4 181 else
komaida424 2:59ac9df97701 182 LCD_printf("Reverse");
komaida424 2:59ac9df97701 183 if ( mode == (GYRODIR+4) )
komaida424 0:cca1c4e84da4 184 Set_Arrow(3);
komaida424 0:cca1c4e84da4 185 else
komaida424 0:cca1c4e84da4 186 Set_Arrow(2);
komaida424 0:cca1c4e84da4 187 break;
komaida424 2:59ac9df97701 188 case ACCELCORRECT:
komaida424 2:59ac9df97701 189 LCD_printf("Acceleration");
komaida424 2:59ac9df97701 190 LCD_locate(2,1);
komaida424 2:59ac9df97701 191 LCD_printf("correction");
komaida424 0:cca1c4e84da4 192 Set_Arrow(1);
komaida424 0:cca1c4e84da4 193 hmax = 3;
komaida424 0:cca1c4e84da4 194 break;
komaida424 2:59ac9df97701 195 case ACCELCORRECT+1:
komaida424 2:59ac9df97701 196 Param_Set_Prompt1("Accel>ROL",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.01f,sw);
komaida424 2:59ac9df97701 197 break;
komaida424 2:59ac9df97701 198 case ACCELCORRECT+2:
komaida424 2:59ac9df97701 199 Param_Set_Prompt1("Accel>PIT",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.01f,sw);
komaida424 2:59ac9df97701 200 break;
komaida424 2:59ac9df97701 201 case ACCELCORRECT+3:
komaida424 2:59ac9df97701 202 Param_Set_Prompt1("Accel>YAW",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.01f,sw);
komaida424 0:cca1c4e84da4 203 break;
komaida424 2:59ac9df97701 204 case PIDSET: //PID Setting
komaida424 2:59ac9df97701 205 LCD_printf("PID Setting");
komaida424 2:59ac9df97701 206 Set_Arrow(1);
komaida424 2:59ac9df97701 207 hmax = 8;
komaida424 2:59ac9df97701 208 break;
komaida424 2:59ac9df97701 209 case PIDSET+1:
komaida424 2:59ac9df97701 210 Param_Set_Prompt1("PID>RoolPitch>kp",&conf.kp[0],2,0.00f,5.00f,0.01f,sw);
komaida424 2:59ac9df97701 211 conf.kp[1] = conf.kp[0];
komaida424 2:59ac9df97701 212 break;
komaida424 2:59ac9df97701 213 case PIDSET+2:
komaida424 2:59ac9df97701 214 Param_Set_Prompt1("PID>RoolPitch>ki",&conf.ki[0],2,0.00f,5.00f,0.01f,sw);
komaida424 2:59ac9df97701 215 conf.ki[1] = conf.ki[0];
komaida424 0:cca1c4e84da4 216 break;
komaida424 2:59ac9df97701 217 case PIDSET+3:
komaida424 2:59ac9df97701 218 Param_Set_Prompt1("PID>RoolPitch>kd",&conf.kd[0],2,0.00f,5.00f,0.01f,sw);
komaida424 2:59ac9df97701 219 conf.kd[1] = conf.kd[0];
komaida424 2:59ac9df97701 220 break;
komaida424 2:59ac9df97701 221 case PIDSET+4:
komaida424 2:59ac9df97701 222 Param_Set_Prompt1("PID>YAW>kp",&conf.kp[2],2,0.00f,5.00f,0.01f,sw);
komaida424 2:59ac9df97701 223 break;
komaida424 2:59ac9df97701 224 case PIDSET+5:
komaida424 2:59ac9df97701 225 Param_Set_Prompt1("PID>YAW>ki",&conf.ki[2],2,0.00f,5.00f,0.01f,sw);
komaida424 2:59ac9df97701 226 break;
komaida424 2:59ac9df97701 227 case PIDSET+6:
komaida424 2:59ac9df97701 228 Param_Set_Prompt1("PID>YAW>kd",&conf.kd[2],2,0.00f,5.00f,0.01f,sw);
komaida424 2:59ac9df97701 229 break;
komaida424 2:59ac9df97701 230 case PIDSET+7:
komaida424 2:59ac9df97701 231 Param_Set_Prompt1("PID Limit",&conf.PID_Limit,2,0.00f,400.00f,10.00f,sw);
komaida424 2:59ac9df97701 232 break;
komaida424 2:59ac9df97701 233 case PIDSET+8:
komaida424 2:59ac9df97701 234 Param_Set_Prompt1("Integral Limit",&conf.Integral_Limit,3,0.00f,100.00f,1.00f,sw);
komaida424 2:59ac9df97701 235 break;
komaida424 2:59ac9df97701 236 case STICKMIX: //Set Stick Mixing
komaida424 2:59ac9df97701 237 LCD_printf("Set Stick Mixing");
komaida424 0:cca1c4e84da4 238 Set_Arrow(1);
komaida424 0:cca1c4e84da4 239 hmax = 3;
komaida424 0:cca1c4e84da4 240 break;
komaida424 2:59ac9df97701 241 case STICKMIX+1: //Set Stick Mixing
komaida424 0:cca1c4e84da4 242 Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 243 break;
komaida424 2:59ac9df97701 244 case STICKMIX+2:
komaida424 0:cca1c4e84da4 245 Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 246 break;
komaida424 2:59ac9df97701 247 case STICKMIX+3:
komaida424 0:cca1c4e84da4 248 Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
komaida424 0:cca1c4e84da4 249 break;
komaida424 2:59ac9df97701 250 case DISPPULSE: //Display Pulse Width
komaida424 2:59ac9df97701 251 LCD_printf("Disp Pulse Width");
komaida424 0:cca1c4e84da4 252 Set_Arrow(1);
komaida424 0:cca1c4e84da4 253 hmax = 2;
komaida424 0:cca1c4e84da4 254 break;
komaida424 2:59ac9df97701 255 case DISPPULSE+1: //Display Pulse Width
komaida424 0:cca1c4e84da4 256 // DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
komaida424 2:59ac9df97701 257 ret_mode = 'R';
komaida424 2:59ac9df97701 258 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 259 sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
komaida424 2:59ac9df97701 260 LCD_printf(str);
komaida424 2:59ac9df97701 261 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 262 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
komaida424 2:59ac9df97701 263 LCD_printf(str);
komaida424 0:cca1c4e84da4 264 break;
komaida424 2:59ac9df97701 265 case DISPPULSE+2: //Display Stick Ref
komaida424 2:59ac9df97701 266 ret_mode = 'R';
komaida424 0:cca1c4e84da4 267 Get_Stick_Pos();
komaida424 2:59ac9df97701 268 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 269 sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
komaida424 2:59ac9df97701 270 LCD_printf(str);
komaida424 2:59ac9df97701 271 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 272 sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
komaida424 2:59ac9df97701 273 LCD_printf(str);
komaida424 0:cca1c4e84da4 274 break;
komaida424 2:59ac9df97701 275 case DISPSENSOR: //Display Sensor Value
komaida424 2:59ac9df97701 276 LCD_printf("Disp Senser");
komaida424 2:59ac9df97701 277 Set_Arrow(1);
komaida424 2:59ac9df97701 278 hmax = 7;
komaida424 2:59ac9df97701 279 for ( i=0; i<3; i++ ) {
komaida424 2:59ac9df97701 280 pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])/50.0
komaida424 2:59ac9df97701 281 ,conf.PID_Limit,conf.Integral_Limit);
komaida424 2:59ac9df97701 282 Angle[i] = 0;
komaida424 2:59ac9df97701 283 }
komaida424 2:59ac9df97701 284 break;
komaida424 2:59ac9df97701 285 case DISPSENSOR+1: //Gyro
komaida424 0:cca1c4e84da4 286 Get_Gyro();
komaida424 2:59ac9df97701 287 LCD_locate(0,0);
komaida424 2:59ac9df97701 288 sprintf(str,"[Gyro]X=%5.1f",Gyro[ROL]);
komaida424 2:59ac9df97701 289 LCD_printf(str);
komaida424 2:59ac9df97701 290 LCD_locate(0,1);
komaida424 2:59ac9df97701 291 sprintf(str,"y=%5.1f,Z=%5.1f",Gyro[PIT],Gyro[YAW]);
komaida424 2:59ac9df97701 292 LCD_printf(str);
komaida424 0:cca1c4e84da4 293 ret_mode = 'R';
komaida424 0:cca1c4e84da4 294 break;
komaida424 2:59ac9df97701 295 case DISPSENSOR+2: //Accelerometer
komaida424 0:cca1c4e84da4 296 Get_Accel();
komaida424 2:59ac9df97701 297 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
komaida424 2:59ac9df97701 298 else i2c.Acceleration(&y,&x,&z);
komaida424 2:59ac9df97701 299 x -= conf.Accel_Ref[0];
komaida424 2:59ac9df97701 300 y -= conf.Accel_Ref[1];
komaida424 2:59ac9df97701 301 z -= conf.Accel_Ref[2];
komaida424 2:59ac9df97701 302 LCD_locate(0,0);
komaida424 2:59ac9df97701 303 sprintf(str,"[Accel]X=%5.1f",x);
komaida424 2:59ac9df97701 304 LCD_printf(str);
komaida424 2:59ac9df97701 305 LCD_locate(0,1);
komaida424 2:59ac9df97701 306 sprintf(str,"Y=%5.1f,Z=%5.1f",y,z);
komaida424 2:59ac9df97701 307 LCD_printf(str);
komaida424 0:cca1c4e84da4 308 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 309 ret_mode = 'R';
komaida424 0:cca1c4e84da4 310 break;
komaida424 2:59ac9df97701 311 case DISPSENSOR+3: //angle
komaida424 2:59ac9df97701 312 PWM_Out(false);
komaida424 2:59ac9df97701 313 LCD_locate(0,0);
komaida424 2:59ac9df97701 314 sprintf(str,"[Angle]X=%6.1f",Angle[ROL]);
komaida424 2:59ac9df97701 315 LCD_printf(str);
komaida424 2:59ac9df97701 316 LCD_locate(0,1);
komaida424 2:59ac9df97701 317 sprintf(str,"Y=%6.1f,Z=%5.1f",Angle[PIT],Angle[YAW]);
komaida424 2:59ac9df97701 318 LCD_printf(str);
komaida424 2:59ac9df97701 319 // Set_Arrow(2);
komaida424 2:59ac9df97701 320 ret_mode = 'R';
komaida424 2:59ac9df97701 321 break;
komaida424 2:59ac9df97701 322 case DISPSENSOR+4:
komaida424 2:59ac9df97701 323 PWM_Out(false);
komaida424 2:59ac9df97701 324 LCD_locate(0,0);
komaida424 2:59ac9df97701 325 sprintf(str,"t=%5d,a=%6.2f",-Stick[YAW]*45/400,Angle[YAW]);
komaida424 2:59ac9df97701 326 LCD_printf(str);
komaida424 2:59ac9df97701 327 LCD_locate(0,1);
komaida424 2:59ac9df97701 328 sprintf(str,"pid=%5d",pid_reg[YAW]);
komaida424 2:59ac9df97701 329 LCD_printf(str);
komaida424 2:59ac9df97701 330 Set_Arrow(2);
komaida424 2:59ac9df97701 331 ret_mode = 'R';
komaida424 2:59ac9df97701 332 break;
komaida424 2:59ac9df97701 333 case DISPSENSOR+5: // Pressure
komaida424 0:cca1c4e84da4 334 elaps.reset();
komaida424 0:cca1c4e84da4 335 elaps.start();
komaida424 0:cca1c4e84da4 336 Get_Pressure();
komaida424 0:cca1c4e84da4 337 elaps.stop();
komaida424 2:59ac9df97701 338 LCD_locate(0,0);
komaida424 2:59ac9df97701 339 sprintf(str,"Press=%9.3f",Press/4096);
komaida424 2:59ac9df97701 340 LCD_printf(str);
komaida424 2:59ac9df97701 341 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 342 sprintf(str,"Elaps=%6d",elaps.read_us());
komaida424 2:59ac9df97701 343 LCD_printf(str);
komaida424 0:cca1c4e84da4 344 // Set_Arrow(2);
komaida424 0:cca1c4e84da4 345 ret_mode = 'R';
komaida424 2:59ac9df97701 346 wait(0.05);
komaida424 0:cca1c4e84da4 347 break;
komaida424 2:59ac9df97701 348 case DISPSENSOR+6:
komaida424 0:cca1c4e84da4 349 elaps.reset();
komaida424 0:cca1c4e84da4 350 elaps.start();
komaida424 0:cca1c4e84da4 351 PWM_Out(false);
komaida424 0:cca1c4e84da4 352 elaps.stop();
komaida424 2:59ac9df97701 353 i = elaps.read_us();
komaida424 2:59ac9df97701 354 LCD_locate(0,0);
komaida424 2:59ac9df97701 355 sprintf(str,"ElapsTime=%6d",i);
komaida424 2:59ac9df97701 356 LCD_printf(str);
komaida424 0:cca1c4e84da4 357 Set_Arrow(2);
komaida424 0:cca1c4e84da4 358 ret_mode = 'R';
komaida424 0:cca1c4e84da4 359 break;
komaida424 2:59ac9df97701 360 case DISPSENSOR+7: //Sensor Calibration
komaida424 0:cca1c4e84da4 361 CalibrateGyros();
komaida424 2:59ac9df97701 362 FlashLED(3);
komaida424 2:59ac9df97701 363 LCD_printf("Calibrate completed");
komaida424 0:cca1c4e84da4 364 Set_Arrow(3);
komaida424 0:cca1c4e84da4 365 break;
komaida424 2:59ac9df97701 366 case DISPPWM: //Display PWM Condition
komaida424 2:59ac9df97701 367 LCD_printf("Display PWM ");
komaida424 0:cca1c4e84da4 368 Set_Arrow(1);
komaida424 0:cca1c4e84da4 369 hmax = 1;
komaida424 0:cca1c4e84da4 370 break;
komaida424 2:59ac9df97701 371 case DISPPWM+1: //Display PWM Width
komaida424 2:59ac9df97701 372 ret_mode = 'R';
komaida424 0:cca1c4e84da4 373 PWM_Out(false);
komaida424 2:59ac9df97701 374 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 375 sprintf(str,"M1=%4d,M2=%4d",M1,M2);
komaida424 2:59ac9df97701 376 LCD_printf(str);
komaida424 2:59ac9df97701 377 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 378 sprintf(str,"M4=%4d,M3=%4d",M4,M3);
komaida424 2:59ac9df97701 379 LCD_printf(str);
komaida424 0:cca1c4e84da4 380 break;
komaida424 2:59ac9df97701 381 case PARMSET: //パラメーター設定
komaida424 2:59ac9df97701 382 LCD_printf("Parameter Set");
komaida424 0:cca1c4e84da4 383 Set_Arrow(1);
komaida424 2:59ac9df97701 384 hmax = 6;
komaida424 0:cca1c4e84da4 385 break;
komaida424 2:59ac9df97701 386 case PARMSET+1:
komaida424 0:cca1c4e84da4 387 Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
komaida424 0:cca1c4e84da4 388 break;
komaida424 2:59ac9df97701 389 case PARMSET+2:
komaida424 2:59ac9df97701 390 LCD_locate(0,0);
komaida424 2:59ac9df97701 391 LCD_printf("PWM>Mode");
komaida424 2:59ac9df97701 392 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 393 switch ( sw ) {
komaida424 0:cca1c4e84da4 394 case 'U':
komaida424 0:cca1c4e84da4 395 case 'D':
komaida424 0:cca1c4e84da4 396 conf.PWM_Mode *= -1;
komaida424 0:cca1c4e84da4 397 }
komaida424 0:cca1c4e84da4 398 if ( conf.PWM_Mode == 1 )
komaida424 2:59ac9df97701 399 LCD_printf("ESC ");
komaida424 0:cca1c4e84da4 400 else
komaida424 2:59ac9df97701 401 LCD_printf("Moter");
komaida424 0:cca1c4e84da4 402 Set_Arrow(2);
komaida424 0:cca1c4e84da4 403 break;
komaida424 2:59ac9df97701 404 case PARMSET+3:
komaida424 0:cca1c4e84da4 405 Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
komaida424 2:59ac9df97701 406 break;
komaida424 2:59ac9df97701 407 case PARMSET+4:
komaida424 2:59ac9df97701 408 Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw);
komaida424 2:59ac9df97701 409 break;
komaida424 2:59ac9df97701 410 case PARMSET+5:
komaida424 2:59ac9df97701 411 Param_Set_Prompt1("ESC>Low Position",&conf.ESC_Low,2,Pulse_Min,Pulse_Max,1,sw);
komaida424 0:cca1c4e84da4 412 break;
komaida424 2:59ac9df97701 413 case PARMSET+6:
komaida424 2:59ac9df97701 414 Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw);
komaida424 0:cca1c4e84da4 415 break;
komaida424 2:59ac9df97701 416 // case PARMSET+7:
komaida424 2:59ac9df97701 417 // Param_Set_Prompt1("Cont. swap Angle",&conf.Control_Exchange_Angle,3,0.0f,40.0f,1.0f,sw);
komaida424 2:59ac9df97701 418 // break;
komaida424 2:59ac9df97701 419 case CONFSTORE: //E2PROM Store
komaida424 2:59ac9df97701 420 LCD_printf("Config Save");
komaida424 0:cca1c4e84da4 421 Set_Arrow(1);
komaida424 0:cca1c4e84da4 422 hmax = 1;
komaida424 0:cca1c4e84da4 423 break;
komaida424 2:59ac9df97701 424 case CONFSTORE+1:
komaida424 0:cca1c4e84da4 425 WriteConfig();
komaida424 2:59ac9df97701 426 LCD_locate(0,0);
komaida424 0:cca1c4e84da4 427 sprintf(str,"Config %3dbyte",sizeof(config));
komaida424 2:59ac9df97701 428 LCD_printf(str);
komaida424 2:59ac9df97701 429 LCD_locate(0,1);
komaida424 2:59ac9df97701 430 LCD_printf("Save Sucssesuful");
komaida424 0:cca1c4e84da4 431 Set_Arrow(3);
komaida424 2:59ac9df97701 432 wait(0.5);
komaida424 2:59ac9df97701 433 FlashLED(5);
komaida424 2:59ac9df97701 434 hnum = 0;
komaida424 0:cca1c4e84da4 435 break;
komaida424 2:59ac9df97701 436 case CONFRESET: //E2PROM reset
komaida424 2:59ac9df97701 437 LCD_printf("Config Reset");
komaida424 2:59ac9df97701 438 Set_Arrow(1);
komaida424 2:59ac9df97701 439 hmax = 3;
komaida424 0:cca1c4e84da4 440 break;
komaida424 2:59ac9df97701 441 case CONFRESET+1:
komaida424 2:59ac9df97701 442 LCD_printf("If want to reset");
komaida424 2:59ac9df97701 443 LCD_locate(0,1);
komaida424 2:59ac9df97701 444 LCD_printf("Aileron right");
komaida424 2:59ac9df97701 445 Set_Arrow(2);
komaida424 2:59ac9df97701 446 break;
komaida424 2:59ac9df97701 447 case CONFRESET+2: //E2PROM reset
komaida424 2:59ac9df97701 448 conf = init;
komaida424 2:59ac9df97701 449 LCD_printf("Rset sucssesuful");
komaida424 2:59ac9df97701 450 Set_Arrow(3);
komaida424 2:59ac9df97701 451 break;
komaida424 0:cca1c4e84da4 452 default:
komaida424 0:cca1c4e84da4 453 if ( hnum == 0 )
komaida424 0:cca1c4e84da4 454 vnum++;
komaida424 0:cca1c4e84da4 455 }
komaida424 0:cca1c4e84da4 456
komaida424 0:cca1c4e84da4 457
komaida424 0:cca1c4e84da4 458 sw = Check_Stick_Dir(ret_mode); //Wait Mode
komaida424 0:cca1c4e84da4 459
komaida424 0:cca1c4e84da4 460 switch ( sw ) {
komaida424 0:cca1c4e84da4 461 case 'L':
komaida424 0:cca1c4e84da4 462 hnum--;
komaida424 0:cca1c4e84da4 463 if ( hnum <= 0 ) hnum = 0;
komaida424 2:59ac9df97701 464 LCD_cls(); //Clear LCD
komaida424 0:cca1c4e84da4 465 break;
komaida424 0:cca1c4e84da4 466 case 'R':
komaida424 2:59ac9df97701 467 LCD_cls();
komaida424 0:cca1c4e84da4 468 if ( hnum < hmax ) hnum++;
komaida424 0:cca1c4e84da4 469 break;
komaida424 0:cca1c4e84da4 470 case 'U':
komaida424 0:cca1c4e84da4 471 if ( hnum == 0 ) {
komaida424 0:cca1c4e84da4 472 if ( vnum < vmax ) vnum++;
komaida424 0:cca1c4e84da4 473 else vnum = 0;
komaida424 2:59ac9df97701 474 LCD_cls(); //Clear LCD
komaida424 0:cca1c4e84da4 475 }
komaida424 0:cca1c4e84da4 476 break;
komaida424 0:cca1c4e84da4 477 case 'D':
komaida424 0:cca1c4e84da4 478 if ( hnum == 0 ) {
komaida424 0:cca1c4e84da4 479 if ( vnum > 0 ) vnum--;
komaida424 0:cca1c4e84da4 480 else vnum = vmax;
komaida424 2:59ac9df97701 481 LCD_cls(); //Clear LCD
komaida424 0:cca1c4e84da4 482 }
komaida424 0:cca1c4e84da4 483 break;
komaida424 0:cca1c4e84da4 484 case 'E':
komaida424 2:59ac9df97701 485 LCD_cls(); //Clear LCD
komaida424 2:59ac9df97701 486 LCD_locate(0,0);
komaida424 2:59ac9df97701 487 LCD_printf("PWM Started");
komaida424 0:cca1c4e84da4 488 return;
komaida424 0:cca1c4e84da4 489 }
komaida424 0:cca1c4e84da4 490 }
komaida424 0:cca1c4e84da4 491
komaida424 0:cca1c4e84da4 492 }
komaida424 0:cca1c4e84da4 493
komaida424 0:cca1c4e84da4 494 char Check_Stick_Dir(char act)
komaida424 0:cca1c4e84da4 495 {
komaida424 0:cca1c4e84da4 496 int i;
komaida424 0:cca1c4e84da4 497 while ( 1 ) {
komaida424 0:cca1c4e84da4 498 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 499 if ( Stick[YAW] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 500 i = 0;
komaida424 0:cca1c4e84da4 501 while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
komaida424 0:cca1c4e84da4 502 if ( i > 2000 ) return 'E'; //wait 2 sec
komaida424 0:cca1c4e84da4 503 wait(0.001); // wait 1 msec
komaida424 0:cca1c4e84da4 504 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 505 i++;
komaida424 0:cca1c4e84da4 506 }
komaida424 0:cca1c4e84da4 507 }
komaida424 0:cca1c4e84da4 508 if ( Stick[ROL] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 509 wait(0.03);
komaida424 0:cca1c4e84da4 510 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 511 if ( !(Stick[ROL] > Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 512 while ( Stick[ROL] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 513 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 514 }
komaida424 0:cca1c4e84da4 515 return 'R';
komaida424 0:cca1c4e84da4 516 }
komaida424 0:cca1c4e84da4 517 if ( Stick[ROL] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 518 wait(0.03);
komaida424 0:cca1c4e84da4 519 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 520 if ( !(Stick[ROL] < -Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 521 while ( Stick[ROL] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 522 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 523 }
komaida424 0:cca1c4e84da4 524 return 'L';
komaida424 0:cca1c4e84da4 525 }
komaida424 0:cca1c4e84da4 526 if ( Stick[PIT] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 527 wait(0.03);
komaida424 0:cca1c4e84da4 528 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 529 if ( !(Stick[PIT] < -Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 530 if ( act == 'R' ) {
komaida424 0:cca1c4e84da4 531 wait(0.03);
komaida424 0:cca1c4e84da4 532 return 'D';
komaida424 0:cca1c4e84da4 533 }
komaida424 0:cca1c4e84da4 534 while ( Stick[PIT] < -Stick_Limit ) {
komaida424 0:cca1c4e84da4 535 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 536 }
komaida424 0:cca1c4e84da4 537 return 'D';
komaida424 0:cca1c4e84da4 538 }
komaida424 0:cca1c4e84da4 539 if ( Stick[PIT] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 540 wait(0.03);
komaida424 0:cca1c4e84da4 541 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 542 if ( !( Stick[PIT] > Stick_Limit) ) continue;
komaida424 0:cca1c4e84da4 543 if ( act == 'R' ) {
komaida424 0:cca1c4e84da4 544 wait(0.03);
komaida424 0:cca1c4e84da4 545 return 'U';
komaida424 0:cca1c4e84da4 546 }
komaida424 0:cca1c4e84da4 547 while ( Stick[PIT] > Stick_Limit ) {
komaida424 0:cca1c4e84da4 548 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 549 }
komaida424 0:cca1c4e84da4 550 return 'U';
komaida424 0:cca1c4e84da4 551 }
komaida424 0:cca1c4e84da4 552 if ( act == 'R' )
komaida424 0:cca1c4e84da4 553 return ' ';
komaida424 0:cca1c4e84da4 554 }
komaida424 0:cca1c4e84da4 555 }
komaida424 0:cca1c4e84da4 556
komaida424 2:59ac9df97701 557 void Param_Set_Prompt1(char *hd,int *num,int arrow,int min,int max,int increase,char sw)
komaida424 0:cca1c4e84da4 558 {
komaida424 0:cca1c4e84da4 559 ret_mode = 'R';
komaida424 2:59ac9df97701 560 LCD_locate(0,0);
komaida424 2:59ac9df97701 561 LCD_printf(hd);
komaida424 2:59ac9df97701 562 LCD_locate(0,1);
komaida424 2:59ac9df97701 563 sprintf(str,"%6d",*num);
komaida424 2:59ac9df97701 564 LCD_printf(str);
komaida424 0:cca1c4e84da4 565 Set_Arrow(arrow);
komaida424 0:cca1c4e84da4 566 switch ( sw ) {
komaida424 0:cca1c4e84da4 567 case 'U':
komaida424 0:cca1c4e84da4 568 *num -= increase;
komaida424 0:cca1c4e84da4 569 if ( *num <= min )
komaida424 0:cca1c4e84da4 570 *num = min;
komaida424 0:cca1c4e84da4 571 break;
komaida424 0:cca1c4e84da4 572 case 'D':
komaida424 0:cca1c4e84da4 573 *num += increase;
komaida424 0:cca1c4e84da4 574 if ( *num >= max )
komaida424 0:cca1c4e84da4 575 *num = max;
komaida424 0:cca1c4e84da4 576 }
komaida424 0:cca1c4e84da4 577 }
komaida424 2:59ac9df97701 578 void Param_Set_Prompt1(char *hd,float *num,int arrow,float min,float max,float increase,char sw)
komaida424 0:cca1c4e84da4 579 {
komaida424 0:cca1c4e84da4 580 ret_mode = 'R';
komaida424 2:59ac9df97701 581 LCD_locate(0,0);
komaida424 2:59ac9df97701 582 LCD_printf(hd);
komaida424 2:59ac9df97701 583 LCD_locate(0,1);
komaida424 0:cca1c4e84da4 584 sprintf(str,"%7.2f",*num);
komaida424 2:59ac9df97701 585 LCD_printf(str);
komaida424 0:cca1c4e84da4 586 Set_Arrow(arrow);
komaida424 0:cca1c4e84da4 587 switch ( sw ) {
komaida424 0:cca1c4e84da4 588 case 'U':
komaida424 0:cca1c4e84da4 589 *num -= increase;
komaida424 0:cca1c4e84da4 590 if ( *num <= min )
komaida424 0:cca1c4e84da4 591 *num = min;
komaida424 0:cca1c4e84da4 592 break;
komaida424 0:cca1c4e84da4 593 case 'D':
komaida424 0:cca1c4e84da4 594 *num += increase;
komaida424 0:cca1c4e84da4 595 if ( *num >= max )
komaida424 0:cca1c4e84da4 596 *num = max;
komaida424 0:cca1c4e84da4 597 }
komaida424 0:cca1c4e84da4 598 }
komaida424 0:cca1c4e84da4 599
komaida424 0:cca1c4e84da4 600 void Set_Arrow(int dir)
komaida424 0:cca1c4e84da4 601 {
komaida424 2:59ac9df97701 602 LCD_locate(12,1);
komaida424 0:cca1c4e84da4 603 switch ( dir ) {
komaida424 0:cca1c4e84da4 604 case 1:
komaida424 2:59ac9df97701 605 LCD_printf(" >>");
komaida424 0:cca1c4e84da4 606 break;
komaida424 0:cca1c4e84da4 607 case 2:
komaida424 2:59ac9df97701 608 LCD_printf("<<>>");
komaida424 0:cca1c4e84da4 609 break;
komaida424 0:cca1c4e84da4 610 case 3:
komaida424 2:59ac9df97701 611 LCD_printf(" <<");
komaida424 0:cca1c4e84da4 612 }
komaida424 0:cca1c4e84da4 613 }
komaida424 0:cca1c4e84da4 614
komaida424 0:cca1c4e84da4 615
komaida424 0:cca1c4e84da4 616
komaida424 0:cca1c4e84da4 617
komaida424 0:cca1c4e84da4 618
komaida424 0:cca1c4e84da4 619
komaida424 2:59ac9df97701 620
komaida424 2:59ac9df97701 621
komaida424 2:59ac9df97701 622
komaida424 2:59ac9df97701 623
komaida424 2:59ac9df97701 624
komaida424 2:59ac9df97701 625
komaida424 2:59ac9df97701 626
komaida424 2:59ac9df97701 627
komaida424 2:59ac9df97701 628
komaida424 2:59ac9df97701 629
komaida424 2:59ac9df97701 630
komaida424 2:59ac9df97701 631
komaida424 2:59ac9df97701 632
komaida424 2:59ac9df97701 633
komaida424 2:59ac9df97701 634
komaida424 2:59ac9df97701 635
komaida424 2:59ac9df97701 636
komaida424 2:59ac9df97701 637
komaida424 2:59ac9df97701 638
komaida424 2:59ac9df97701 639
komaida424 2:59ac9df97701 640
komaida424 2:59ac9df97701 641
komaida424 2:59ac9df97701 642
komaida424 2:59ac9df97701 643
komaida424 2:59ac9df97701 644
komaida424 2:59ac9df97701 645
komaida424 2:59ac9df97701 646
komaida424 2:59ac9df97701 647
komaida424 2:59ac9df97701 648
komaida424 2:59ac9df97701 649
komaida424 2:59ac9df97701 650
komaida424 2:59ac9df97701 651
komaida424 2:59ac9df97701 652
komaida424 2:59ac9df97701 653