syouichi imamori / Mbed OS 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.

Dependencies:   IAP

config.cpp@3:27407c4984cf, 2014-02-13 (annotated)

Committer:
komaida424
Date:
Thu Feb 13 16:07:07 2014 +0000
Revision:
3:27407c4984cf
Parent:
2:59ac9df97701
Child:
4:4060309b9cc0
revsion.1.30

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