syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

config.cpp@8:1db19b529b22, 2021-02-21 (annotated)

Committer:
komaida424
Date:
Sun Feb 21 05:14:57 2021 +0000
Revision:
8:1db19b529b22
Parent:
6:a50e6d3924f1 
rev 020

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