航空研究会
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
AutoFlight2018_Control
by 
航空研究会
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //mbed 00002 #include "mbed.h" 00003 #include "FATFileSystem.h" 00004 #include "SDFileSystem.h" 00005 //C 00006 #include "math.h" 00007 //sensor 00008 #include "MPU6050_DMP6.h" 00009 //#include "MPU9250.h" 00010 //#include "BMP280.h" 00011 #include "hcsr04.h" 00012 //device 00013 #include "sbus.h" 00014 //config 00015 #include "SkipperSv2.h" 00016 #include "falfalla.h" 00017 //other 00018 #include "pid.h" 00019 00020 #define DEBUG_SEMIAUTO 0 00021 #define DEBUG_PRINT_INLOOP 00022 00023 #define KP_ELE 2.0 00024 #define KI_ELE 0.0 00025 #define KD_ELE 0.0 00026 #define KP_RUD 8.0 00027 #define KI_RUD 0.0 00028 #define KD_RUD 0.0 00029 00030 #define GAIN_CONTROLVALUE_TO_PWM 3.0 00031 00032 #define RIGHT_ROLL -17.0 00033 #define RIGHT_PITCH -6.3 00034 #define LEFT_ROLL 16.0 00035 #define LEFT_PITCH -6.0 00036 #define STRAIGHT_ROLL 2.0 00037 #define STRAIGHT_PITCH -3.0 00038 #define TAKEOFF_THR 1.0 00039 #define LOOP_THR 0.58 00040 00041 #define RIGHT_ROLL_SHORT -20.0 00042 #define RIGHT_PITCH_SHORT -7.0 00043 #define LEFT_ROLL_SHORT 22.0 00044 #define LEFT_PITCH_SHORT -6.0 00045 00046 #define GLIDE_ROLL 16.0 00047 #define GLIDE_PITCH 0.0 00048 00049 //コンパスキャリブレーション 00050 //SkipperS2基板 00051 /* 00052 #define MAGBIAS_X -35.0 00053 #define MAGBIAS_Y 535.0 00054 #define MAGBIAS_Z -50.0 00055 */ 00056 //S2v2 1番基板 00057 #define MAGBIAS_X 395.0 00058 #define MAGBIAS_Y 505.0 00059 #define MAGBIAS_Z -725.0 00060 //S2v2 2番基板 00061 /* 00062 #define MAGBIAS_X 185.0 00063 #define MAGBIAS_Y 220.0 00064 #define MAGBIAS_Z -350.0 00065 */ 00066 00067 #define ELEMENT 1 00068 #define LIMIT_STRAIGHT_YAW 5.0 00069 #define LIMIT_LOOPSTOP_YAW 1.0 00070 #define THRESHOLD_TURNINGRADIUS_YAW 60.0 00071 #define ALLOWHEIGHT 15 00072 00073 #ifndef PI 00074 #define PI 3.14159265358979 00075 #endif 00076 00077 const int16_t lengthdivpwm = 320; 00078 const int16_t changeModeCount = 6; 00079 00080 const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]}; 00081 const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]}; 00082 const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]}; 00083 const int16_t reverce[4] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; 00084 00085 SBUS sbus(PA_9, PA_10); //SBUS 00086 00087 //PwmOut servo1(PC_6); // TIM3_CH1 //echo 00088 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 00089 PwmOut servo3(PB_0); // TIM3_CH3 00090 PwmOut servo4(PB_1); // TIM3_CH4 00091 PwmOut servo5(PB_6); // TIM4_CH1 00092 //PwmOut servo6(PB_7); // TIM4_CH2 //trigger 00093 //PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 00094 //PwmOut servo8(PB_9); // TIM4_CH4 00095 00096 RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX 00097 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); 00098 00099 DigitalOut led1(PA_0); //黄色のコネクタ 00100 DigitalOut led2(PA_1); 00101 DigitalOut led3(PB_4); 00102 DigitalOut led4(PB_5); 00103 00104 //InterruptIn switch2(PC_14); 00105 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine 00106 HCSR04 usensor(PB_7,PC_6); //trig,echo 9,8 00107 00108 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); 00109 PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD); 00110 00111 enum Channel{AIL, ELE, THR, RUD, DROP, Ch6, Ch7, Ch8}; 00112 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 00113 enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide}; 00114 enum BombingMode{Takeoff, Chicken, Transition, Approach}; 00115 enum OutputStatus{Manual, Auto}; 00116 00117 static OutputStatus output_status = Manual; 00118 OperationMode operation_mode = StartUp; 00119 BombingMode bombing_mode = Takeoff; 00120 static int16_t autopwm[8] = {1500,1500,1180,1500,1446,1500}; 00121 static int16_t trimpwm[6] = {1500,1500,1180,1500,1446,1500}; 00122 int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820}; 00123 int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180}; 00124 int16_t oldTHR = 1000; 00125 00126 static float nowAngle[3] = {0,0,0}; 00127 const float trimAngle[3] = {0.0, 0.0, 0.0}; 00128 const float maxAngle[2] = {90, 90}; 00129 const float minAngle[2] = {-90, -90}; 00130 00131 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; 00132 00133 unsigned int g_distance; 00134 Ticker USsensor; 00135 static char g_buf[16]; 00136 char g_landingcommand; 00137 float g_SerialTargetYAW = 0.0; 00138 00139 Timer t; 00140 Timeout RerurnChickenServo1; 00141 Timeout RerurnChickenServo2; 00142 00143 /*-----関数のプロトタイプ宣言-----*/ 00144 void setup(); 00145 void loop(); 00146 00147 void Init_PWM(); 00148 void Init_servo(); //サーボ初期化 00149 void Init_sbus(); //SBUS初期化 00150 void Init_sensors(); 00151 void DisplayClock(); //クロック状態確認 00152 00153 //センサの値取得 00154 void SensingMPU(); 00155 void UpdateDist(); 00156 00157 //割り込み処理 00158 void ISR_Serial_Rx(); 00159 00160 //void offsetRollPitch(float FirstROLL, float FirstPITCH); 00161 //void TransYaw(float FirstYAW); 00162 float TranslateNewYaw(float beforeYaw, float newzeroYaw); 00163 void UpdateTargetAngle(float targetAngle[3]); 00164 void CalculateControlValue(float targetAngle[3], float controlValue[3]); 00165 void UpdateAutoPWM(float controlValue[3]); 00166 void ConvertPWMintoRAD(float targetAngle[3]); 00167 inline float CalcRatio(float value, float trim, float limit); 00168 bool CheckSW_Up(Channel ch); 00169 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min); 00170 inline int16_t SetTHRinRatio(float ratio); 00171 00172 //sbus割り込み 00173 void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 00174 void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力 00175 00176 //シリアル割り込み 00177 void SendSerial(); //1文字きたら送り返す 00178 void SendArray(); 00179 void getSF_Serial(); 00180 float ConvertByteintoFloat(char high, char low); 00181 00182 //SD設定 00183 int GetParameter(FILE *fp, const char *paramName,char parameter[]); 00184 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, 00185 float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, 00186 float *g_rightloopROLL, float *g_rightloopPITCH, 00187 float *g_leftloopROLL, float *g_leftloopPITCH, 00188 float *g_gostraightROLL, float *g_gostraightPITCH, 00189 float *g_takeoffTHR, float *g_loopTHR, 00190 float *g_rightloopROLLshort, float *g_rightloopPITCHshort, 00191 float *g_leftloopROLLshort, float *g_leftloopPITCHshort, 00192 float *g_glideloopROLL, float *g_glideloopPITCH); 00193 //switch2割り込み 00194 void ResetTrim(); 00195 00196 //自動操縦 00197 void UpdateTargetAngle_GoStraight(float targetAngle[3]); 00198 void UpdateTargetAngle_Rightloop(float targetAngle[3]); 00199 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); 00200 void UpdateTargetAngle_Leftloop(float targetAngle[3]); 00201 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); 00202 void UpdateTargetAngle_Moebius(float targetAngle[3]); 00203 void UpdateTargetAngle_Glide(float targetAngle[3]); 00204 void UpdateTargetAngle_Takeoff(float targetAngle[3]); 00205 void UpdateTargetAngle_Approach(float targetAngle[3]); 00206 void Take_off_and_landing(float targetAngle[3]); 00207 00208 char Rotate(float targetAngle[3], float TargetYAW, char mode); 00209 00210 //投下 00211 void Chicken_Drop(); 00212 void ReturnChickenServo1(); 00213 void ReturnChickenServo2(); 00214 00215 //超音波による高度補正 00216 void checkHeight(float targetAngle[3]); 00217 void UpdateTargetAngle_NoseUP(float targetAngle[3]); 00218 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]); 00219 00220 //デバッグ用 00221 void DebugPrint(); 00222 00223 /*---関数のプロトタイプ宣言終わり---*/ 00224 00225 int main() 00226 { 00227 setup(); 00228 00229 while(1){ 00230 00231 loop(); 00232 00233 if(!CheckSW_Up(Ch7)){ 00234 led3=0; 00235 }else{ 00236 led3=1; 00237 } 00238 } 00239 } 00240 00241 void setup(){ 00242 //buzzer = 0; 00243 led1 = 1; 00244 led2 = 1; 00245 led3 = 1; 00246 led4 = 1; 00247 00248 SetOptions(&g_kpELE, &g_kiELE, &g_kdELE, 00249 &g_kpRUD, &g_kiRUD, &g_kdRUD, 00250 &g_rightloopROLL, &g_rightloopPITCH, 00251 &g_leftloopROLL, &g_leftloopPITCH, 00252 &g_gostraightROLL, &g_gostraightPITCH, 00253 &g_takeoffTHR, &g_loopTHR, 00254 &g_rightloopROLLshort, &g_rightloopPITCHshort, 00255 &g_leftloopROLLshort, &g_leftloopPITCHshort, 00256 &g_glideloopROLL, &g_glideloopPITCH); 00257 00258 00259 Init_PWM(); 00260 Init_servo(); 00261 Init_sbus(); 00262 Init_sensors(); 00263 //switch2.rise(ResetTrim); 00264 pc.attach(getSF_Serial, Serial::RxIrq); 00265 USsensor.attach(&UpdateDist, 0.05); 00266 00267 NVIC_SetPriority(USART1_IRQn,0); 00268 NVIC_SetPriority(EXTI0_IRQn,1); 00269 NVIC_SetPriority(TIM5_IRQn,2); 00270 NVIC_SetPriority(EXTI9_5_IRQn,3); 00271 NVIC_SetPriority(USART2_IRQn,4); 00272 DisplayClock(); 00273 t.start(); 00274 00275 00276 pc.printf("MPU calibration start\r\n"); 00277 00278 float offsetstart = t.read(); 00279 while(t.read() - offsetstart < 26){ 00280 SensingMPU(); 00281 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); 00282 pc.printf("\r\n"); 00283 led1 = !led1; 00284 led2 = !led2; 00285 led3 = !led3; 00286 led4 = !led4; 00287 } 00288 00289 FirstROLL = nowAngle[ROLL]; 00290 FirstPITCH = nowAngle[PITCH]; 00291 nowAngle[ROLL] -=FirstROLL; 00292 nowAngle[PITCH] -=FirstPITCH; 00293 00294 led1 = 0; 00295 led2 = 0; 00296 led3 = 0; 00297 led4 = 0; 00298 wait(0.2); 00299 00300 00301 pc.printf("All initialized\r\n"); 00302 } 00303 00304 void loop(){ 00305 static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; 00306 00307 SensingMPU(); 00308 UpdateTargetAngle(targetAngle); 00309 //Rotate(targetAngle, 30.0); 00310 CalculateControlValue(targetAngle, controlValue); 00311 UpdateAutoPWM(controlValue); 00312 wait_ms(23); 00313 //#if DEBUG_PRINT_INLOOP 00314 DebugPrint(); 00315 //#endif 00316 } 00317 00318 //サーボ初期化関数 00319 void Init_servo(){ 00320 00321 //servo1.period_ms(14); 00322 //servo1.pulsewidth_us(trimpwm[AIL]); 00323 00324 servo2.period_ms(14); 00325 servo2.pulsewidth_us(trimpwm[ELE]); 00326 00327 servo3.period_ms(14); 00328 servo3.pulsewidth_us(trimpwm[THR]); 00329 00330 servo4.period_ms(14); 00331 servo4.pulsewidth_us(trimpwm[RUD]); 00332 00333 servo5.period_ms(14); 00334 servo5.pulsewidth_us(1392); 00335 00336 pc.printf("servo initialized\r\n"); 00337 } 00338 00339 //Sbus初期化 00340 void Init_sbus(){ 00341 sbus.initialize(); 00342 sbus.setLastfuncPoint(Update_PWM); 00343 sbus.startInterrupt(); 00344 } 00345 00346 void Init_sensors(){ 00347 if(mpu6050.setup() == -1){ 00348 pc.printf("failed initialize\r\n"); 00349 while(1){ 00350 led1 = 1; led2 = 0; led3 = 1; led4 = 0; 00351 wait(1); 00352 led1 = 0; led2 = 1; led3 = 0; led4 = 1; 00353 wait(1); 00354 } 00355 } 00356 } 00357 00358 void Init_PWM(){ 00359 for (uint8_t i = 0; i < 4; ++i){ 00360 trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100)); 00361 maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100)); 00362 minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100)); 00363 } 00364 pc.printf("PWM initialized\r\n"); 00365 } 00366 00367 void DisplayClock(){ 00368 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); 00369 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); 00370 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); 00371 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); 00372 pc.printf("\r\n"); 00373 } 00374 00375 void UpdateTargetAngle(float targetAngle[3]){ 00376 static int16_t count_op = 0, count_out = 0; 00377 #if DEBUG_SEMIAUTO 00378 switch(operation_mode){ 00379 case StartUp: 00380 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00381 count_op++; 00382 if(count_op > changeModeCount){ 00383 operation_mode = SemiAuto; 00384 pc.printf("Goto SemiAuto mode\r\n"); 00385 count_op = 0; 00386 } 00387 }else count_op = 0; 00388 break; 00389 00390 case SemiAuto: 00391 /* 大会用では以下のif文を入れてoperation_modeを変える 00392 if(CheckSW_Up(Ch6)){ 00393 count_op++; 00394 if(count_op>changeModeCount){ 00395 output_status = XXX; 00396 led2 = 0; 00397 pc.printf("Goto XXX mode\r\n"); 00398 count_op = 0; 00399 }else count_op = 0; 00400 ConvertPWMintoRAD(targetAngle); 00401 } 00402 */ 00403 ConvertPWMintoRAD(targetAngle); 00404 break; 00405 00406 default: 00407 operation_mode = SemiAuto; 00408 break; 00409 } 00410 00411 #else 00412 00413 switch(operation_mode){ 00414 case StartUp: 00415 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え ch8;自動操縦モード切替 00416 count_op++; 00417 if(count_op > changeModeCount){ 00418 operation_mode = LeftLoop; 00419 pc.printf("Goto LeftLoop mode\r\n"); 00420 count_op = 0; 00421 } 00422 }else count_op = 0; 00423 break; 00424 00425 case LeftLoop: 00426 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00427 count_op++; 00428 if(count_op > changeModeCount){ 00429 operation_mode = RightLoop; 00430 pc.printf("Goto RightLoop mode\r\n"); 00431 count_op = 0; 00432 } 00433 }else count_op = 0; 00434 UpdateTargetAngle_Leftloop(targetAngle); 00435 break; 00436 00437 case RightLoop: 00438 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00439 count_op++; 00440 if(count_op > changeModeCount){ 00441 operation_mode = Moebius; 00442 pc.printf("Goto Moebius mode\r\n"); 00443 count_op = 0; 00444 } 00445 }else count_op = 0; 00446 UpdateTargetAngle_Rightloop(targetAngle); 00447 00448 //Rotate確認用 00449 /* 00450 static char mode = 'G'; 00451 mode = Rotate(targetAngle,g_SerialTargetYAW,mode); 00452 */ 00453 break; 00454 00455 case Moebius: 00456 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00457 count_op++; 00458 if(count_op > changeModeCount){ 00459 operation_mode = Glide; 00460 pc.printf("Goto Glide mode\r\n"); 00461 count_op = 0; 00462 } 00463 }else count_op = 0; 00464 UpdateTargetAngle_Moebius(targetAngle); 00465 break; 00466 00467 case Glide: 00468 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00469 count_op++; 00470 if(count_op > changeModeCount){ 00471 operation_mode = BombwithPC; 00472 pc.printf("Goto Bombing mode\r\n"); 00473 count_op = 0; 00474 } 00475 }else count_op = 0; 00476 UpdateTargetAngle_Glide(targetAngle); 00477 break; 00478 00479 case BombwithPC: 00480 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00481 count_op++; 00482 if(count_op > changeModeCount){ 00483 operation_mode = GoStraight; 00484 pc.printf("Goto GoStraight mode\r\n"); 00485 count_op = 0; 00486 } 00487 }else count_op = 0; 00488 Take_off_and_landing(targetAngle); 00489 break; 00490 00491 case GoStraight: 00492 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00493 count_op++; 00494 if(count_op > changeModeCount){ 00495 operation_mode = LeftLoop; 00496 pc.printf("Goto Leftloop mode\r\n"); 00497 count_op = 0; 00498 } 00499 }else count_op = 0; 00500 UpdateTargetAngle_GoStraight(targetAngle); 00501 break; 00502 00503 00504 default: 00505 00506 operation_mode = StartUp; 00507 break; 00508 } 00509 #endif 00510 00511 if(CheckSW_Up(Ch7) && output_status == Manual){ 00512 count_out++; 00513 if(count_out > changeModeCount){ 00514 output_status = Auto; 00515 //led3 = 1; 00516 count_out = 0; 00517 } 00518 }else if(!CheckSW_Up(Ch7) && output_status == Auto){ 00519 count_out++; 00520 if(count_out > changeModeCount){ 00521 output_status = Manual; 00522 count_out = 0; 00523 //led3 = 0; 00524 } 00525 }else count_out = 0; 00526 } 00527 00528 int GetParameter(FILE *fp, const char *paramName,char parameter[]){ 00529 int i=0, j=0; 00530 int strmax = 200; 00531 char str[strmax]; 00532 00533 rewind(fp); //ファイル位置を先頭に 00534 while(1){ 00535 if (fgets(str, strmax, fp) == NULL) { 00536 return 0; 00537 } 00538 if (!strncmp(str, paramName, strlen(paramName))) { 00539 while (str[i++] != '=') {} 00540 while (str[i] != '\n') { 00541 parameter[j++] = str[i++]; 00542 } 00543 parameter[j] = '\0'; 00544 return 1; 00545 } 00546 } 00547 return 0; 00548 } 00549 00550 00551 //sdによる設定 00552 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, 00553 float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, 00554 float *g_rightloopROLL, float *g_rightloopPITCH, 00555 float *g_leftloopROLL, float *g_leftloopPITCH, 00556 float *g_gostraightROLL, float *g_gostraightPITCH, 00557 float *g_takeoffTHR, float *g_loopTHR, 00558 float *g_rightloopROLLshort, float *g_rightloopPITCHshort, 00559 float *g_leftloopROLLshort, float *g_leftloopPITCHshort, 00560 float *g_glideloopROLL, float *g_glideloopPITCH 00561 ){ 00562 00563 pc.printf("SDsetup start.\r\n"); 00564 00565 FILE *fp; 00566 char parameter[30]; //文字列渡す用の配列 00567 int SDerrorcount = 0; //取得できなかった数を返す 00568 const char *paramNames[] = { 00569 "KP_ELEVATOR", 00570 "KI_ELEVATOR", 00571 "KD_ELEVATOR", 00572 "KP_RUDDER", 00573 "KI_RUDDER", 00574 "KD_RUDDER", 00575 "RIGHTLOOP_ROLL", 00576 "RIGHTLOOP_PITCH", 00577 "LEFTLOOP_ROLL", 00578 "LEFTLOOP_PITCH", 00579 "GOSTRAIGHT_ROLL", 00580 "GOSTRAIGHT_PITCH", 00581 "TAKEOFF_THR_RATE", 00582 "LOOP_THR_RATE", 00583 "RIGHTLOOP_ROLL_SHORT", 00584 "RIGHTLOOP_PITCH_SHORT", 00585 "LEFTLOOP_ROLL_SHORT", 00586 "LEFTLOOP_PITCH_SHORT", 00587 "AUTOGLIDE_ROLL", 00588 "AUTOGLIDE PITCH" 00589 }; 00590 00591 fp = fopen("/sd/option.txt","r"); 00592 00593 if(fp != NULL){ //開けたら 00594 pc.printf("File was openned.\r\n"); 00595 if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter); 00596 else{ *g_kpELE = KP_ELE; 00597 SDerrorcount++; 00598 } 00599 if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter); 00600 else{ *g_kiELE = KI_ELE; 00601 SDerrorcount++; 00602 } 00603 if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter); 00604 else{ *g_kdELE = KD_ELE; 00605 SDerrorcount++; 00606 } 00607 if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter); 00608 else{ *g_kpRUD = KP_RUD; 00609 SDerrorcount++; 00610 } 00611 if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter); 00612 else{ *g_kiRUD = KI_RUD; 00613 SDerrorcount++; 00614 } 00615 if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter); 00616 else{ *g_kdRUD = KD_RUD; 00617 SDerrorcount++; 00618 } 00619 if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter); 00620 else{ *g_rightloopROLL = RIGHT_ROLL; 00621 SDerrorcount++; 00622 } 00623 if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter); 00624 else{ *g_rightloopPITCH = RIGHT_PITCH; 00625 SDerrorcount++; 00626 } 00627 if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter); 00628 else{ *g_leftloopROLL = LEFT_ROLL; 00629 SDerrorcount++; 00630 } 00631 if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter); 00632 else{ *g_leftloopPITCH = LEFT_PITCH; 00633 SDerrorcount++; 00634 } 00635 if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter); 00636 else{ *g_gostraightROLL = STRAIGHT_ROLL; 00637 SDerrorcount++; 00638 } 00639 if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter); 00640 else{ *g_gostraightPITCH = STRAIGHT_PITCH; 00641 SDerrorcount++; 00642 } 00643 if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter); 00644 else{ *g_takeoffTHR = TAKEOFF_THR; 00645 SDerrorcount++; 00646 } 00647 if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter); 00648 else{ *g_loopTHR = LOOP_THR; 00649 SDerrorcount++; 00650 } 00651 if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter); 00652 else{ *g_rightloopROLLshort = RIGHT_ROLL_SHORT; 00653 SDerrorcount++; 00654 } 00655 if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter); 00656 else{ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT; 00657 SDerrorcount++; 00658 } 00659 if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter); 00660 else{ *g_leftloopROLLshort = LEFT_ROLL_SHORT; 00661 SDerrorcount++; 00662 } 00663 if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter); 00664 else{ *g_leftloopPITCHshort = LEFT_PITCH_SHORT; 00665 SDerrorcount++; 00666 } 00667 if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter); 00668 else{ *g_glideloopROLL = GLIDE_ROLL; 00669 SDerrorcount++; 00670 } 00671 if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter); 00672 else{ *g_glideloopPITCH = GLIDE_PITCH; 00673 SDerrorcount++; 00674 } 00675 fclose(fp); 00676 00677 }else{ //ファイルがなかったら 00678 pc.printf("fp was null.\r\n"); 00679 *g_kpELE = KP_ELE; 00680 *g_kiELE = KI_ELE; 00681 *g_kdELE = KD_ELE; 00682 *g_kpRUD = KP_RUD; 00683 *g_kiRUD = KI_RUD; 00684 *g_kdRUD = KD_RUD; 00685 *g_rightloopROLL = RIGHT_ROLL; 00686 *g_rightloopPITCH = RIGHT_PITCH; 00687 *g_leftloopROLL = LEFT_ROLL; 00688 *g_leftloopPITCH = LEFT_PITCH; 00689 *g_gostraightROLL = STRAIGHT_ROLL; 00690 *g_gostraightPITCH = STRAIGHT_PITCH; 00691 *g_takeoffTHR = TAKEOFF_THR; 00692 *g_loopTHR = LOOP_THR; 00693 SDerrorcount = -1; 00694 } 00695 pc.printf("SDsetup finished.\r\n"); 00696 if(SDerrorcount == 0) pc.printf("setting option is success\r\n"); 00697 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n"); 00698 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 00699 00700 pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD); 00701 pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE); 00702 pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH); 00703 pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", *g_leftloopROLL, *g_leftloopPITCH); 00704 pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH); 00705 pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR); 00706 pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort); 00707 pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort); 00708 pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH); 00709 00710 return SDerrorcount; 00711 } 00712 00713 void CalculateControlValue(float targetAngle[3], float controlValue[3]){ 00714 static int t_last; 00715 int t_now; 00716 float dt; 00717 00718 t_now = t.read_us(); 00719 dt = (float)((t_now - t_last)/1000000.0f) ; 00720 t_last = t_now; 00721 00722 controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); 00723 controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); 00724 } 00725 00726 void UpdateAutoPWM(float controlValue[3]){ 00727 int16_t addpwm[2]; //-500~500 00728 addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正 00729 addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！） レバー:右回転正 00730 00731 autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; 00732 autopwm[RUD] = trimpwm[RUD] + reverce[RUD] * addpwm[ROLL]; 00733 //autopwm[THR] = oldTHR; 00734 00735 autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); 00736 autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]); 00737 } 00738 00739 inline float CalcRatio(float value, float trim, float limit){ 00740 return (value - trim) / (limit - trim); 00741 } 00742 00743 bool CheckSW_Up(Channel ch){ 00744 if(SWITCH_CHECK < sbus.manualpwm[ch]){ 00745 return true; 00746 }else{ 00747 return false; 00748 } 00749 } 00750 00751 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){ 00752 if(value > max) return max; 00753 if(value < min) return min; 00754 return value; 00755 } 00756 00757 inline int16_t SetTHRinRatio(float ratio){ 00758 return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); 00759 } 00760 00761 /*---SBUS割り込み処理---*/ 00762 00763 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) 00764 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) 00765 void Update_PWM() 00766 { 00767 static int16_t pwm[5], sbuspwm[5]; 00768 static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]}; 00769 static int16_t tempsbuspwm[5] = {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]}; 00770 static int counter_abnormalpwm=0; 00771 if(sbus.flg_ch_update == true){ 00772 switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 00773 case Manual: 00774 for(uint8_t i=1;i<4;i++){ 00775 if(abs(sbus.manualpwm[i]-tempsbuspwm[i])>300){ 00776 counter_abnormalpwm++; 00777 if(counter_abnormalpwm<=1) sbuspwm[i]=tempsbuspwm[i]; 00778 else { 00779 counter_abnormalpwm = 0; 00780 sbuspwm[i] = sbus.manualpwm[i]; 00781 } 00782 } 00783 else{ 00784 counter_abnormalpwm = 0; 00785 sbuspwm[i] = sbus.manualpwm[i]; 00786 } 00787 tempsbuspwm[i]=sbus.manualpwm[i]; 00788 } 00789 sbuspwm[4] = sbus.manualpwm[4]; 00790 00791 for(uint8_t i=1;i<5;i++){ 00792 pwm[i] = sbuspwm[i]; 00793 } 00794 oldTHR = sbuspwm[THR]; 00795 00796 //pc.printf("update_manual\r\n"); 00797 break; 00798 00799 case Auto: 00800 pwm[ELE] = autopwm[ELE]; 00801 pwm[THR] = autopwm[THR]; 00802 pwm[RUD] = autopwm[RUD]; 00803 pwm[DROP] = autopwm[DROP]; 00804 //pc.printf("update_auto\r\n"); 00805 break; 00806 00807 default: 00808 for(uint8_t i=1;i<5;i++){ 00809 pwm[i] = sbus.manualpwm[i]; 00810 } //pc.printf("update_manual\r\n"); 00811 break; 00812 } 00813 for(uint8_t i=1;i<5;i++) if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; 00814 00815 if(pwm[4]>1600 || pwm[4]<1420) pwm[4]=temppwm[4]; 00816 temppwm[4] = pwm[4]; 00817 00818 }else{ 00819 pc.printf("0\r\n"); 00820 } 00821 sbus.flg_ch_update = false; 00822 00823 Output_PWM(pwm); 00824 } 00825 00826 00827 //pwmをサーボに出力。 00828 void Output_PWM(int16_t pwm[5]) 00829 { 00830 //servo1.pulsewidth_us(pwm[0]); 00831 servo2.pulsewidth_us(pwm[1]); 00832 servo3.pulsewidth_us(pwm[2]); 00833 servo4.pulsewidth_us(pwm[3]); 00834 servo5.pulsewidth_us(pwm[4]); 00835 } 00836 00837 void ResetTrim(){ 00838 for(uint8_t i=1; i<4; i++){ 00839 trimpwm[i] = sbus.manualpwm[i]; 00840 } 00841 pc.printf("reset PWM trim\r\n"); 00842 } 00843 00844 00845 void SensingMPU(){ 00846 //static int16_t deltaT = 0, t_start = 0; 00847 //t_start = t.read_us(); 00848 00849 float rpy[3] = {0}, oldrpy[3] = {0}; 00850 static uint16_t count_changeRPY = 0; 00851 static bool flg_checkoutlier = false; 00852 NVIC_DisableIRQ(USART1_IRQn); 00853 NVIC_DisableIRQ(USART2_IRQn); 00854 NVIC_DisableIRQ(TIM5_IRQn); 00855 NVIC_DisableIRQ(EXTI0_IRQn); 00856 NVIC_DisableIRQ(EXTI9_5_IRQn); 00857 00858 mpu6050.getRollPitchYaw_Skipper(rpy); 00859 00860 NVIC_EnableIRQ(USART1_IRQn); 00861 NVIC_EnableIRQ(USART2_IRQn); 00862 NVIC_EnableIRQ(TIM5_IRQn); 00863 NVIC_EnableIRQ(EXTI0_IRQn); 00864 NVIC_EnableIRQ(EXTI9_5_IRQn); 00865 00866 00867 //外れ値対策 00868 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; 00869 rpy[ROLL] -= FirstROLL; 00870 rpy[PITCH] -= FirstPITCH; 00871 rpy[YAW] -= FirstYAW; 00872 00873 for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}} 00874 if(!flg_checkoutlier || count_changeRPY >= 2){ 00875 for(uint8_t i=0; i<3; i++){ 00876 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 00877 } 00878 count_changeRPY = 0; 00879 }else count_changeRPY++; 00880 flg_checkoutlier = false; 00881 00882 } 00883 00884 float TranslateNewYaw(float beforeYaw, float newzeroYaw){ 00885 float newYaw = beforeYaw - newzeroYaw; 00886 00887 if(newYaw<-180.0f) newYaw += 360.0f; 00888 else if(newYaw>180.0f) newYaw -= 360.0f; 00889 return newYaw; 00890 } 00891 00892 00893 void getSF_Serial(){ 00894 00895 static char SFbuf[16]; 00896 static int bufcounter=0; 00897 00898 SFbuf[bufcounter]=pc.getc(); 00899 if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++; 00900 00901 if(bufcounter==5 && SFbuf[4]=='F'){ 00902 g_landingcommand = SFbuf[1]; 00903 if(g_landingcommand=='Y' || g_landingcommand=='C')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); 00904 bufcounter = 0; 00905 memset(SFbuf, 0, strlen(SFbuf)); 00906 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 00907 } 00908 else if(bufcounter>=5 && SFbuf[4]!='F'){ 00909 //pc.printf("Communication Falsed.\r\n"); 00910 bufcounter = 0; 00911 } 00912 } 00913 00914 float ConvertByteintoFloat(char high, char low){ 00915 00916 //int16_t intvalue = (int16_t)high*256 + (int16_t)low; 00917 int16_t intvalue = (int16_t)(((int16_t)high << 8) | low); // Turn the MSB and LSB into a signed 16-bit value 00918 float floatvalue = (float)intvalue; 00919 return floatvalue; 00920 } 00921 00922 00923 //超音波割り込み 00924 void UpdateDist(){ 00925 g_distance = usensor.get_dist_cm(); 00926 usensor.start(); 00927 } 00928 00929 //8の字旋回 00930 void UpdateTargetAngle_Moebius(float targetAngle[3]){ 00931 static uint8_t RotateCounter=0; 00932 static uint8_t precounter=0; 00933 static bool flg_setInStartAuto = false; 00934 static float FirstYAW_Moebius = 0.0; 00935 float newYaw_Moebius; 00936 00937 if(!flg_setInStartAuto && CheckSW_Up(Ch7)){ 00938 FirstYAW_Moebius = nowAngle[YAW]; 00939 RotateCounter = 0; 00940 flg_setInStartAuto = true; 00941 }else if(!CheckSW_Up(Ch7)){ 00942 flg_setInStartAuto = false; 00943 led2 = 0; 00944 } 00945 00946 newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); 00947 00948 if(RotateCounter == 0 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){ 00949 precounter++; 00950 if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;} 00951 } 00952 if(RotateCounter == 1 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){ 00953 precounter++; 00954 if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;} 00955 00956 } 00957 if(RotateCounter == 2 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){ 00958 precounter++; 00959 if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;} 00960 } 00961 if(RotateCounter == 3 && newYaw_Moebius >10.0f && newYaw_Moebius < 90.0f){ 00962 precounter++; 00963 if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;} 00964 } 00965 if(RotateCounter == 4 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){ 00966 precounter++; 00967 if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;} 00968 } 00969 if(RotateCounter == 5 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){ 00970 precounter++; 00971 if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;} 00972 } 00973 if(RotateCounter == 6 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){ 00974 precounter++; 00975 if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;} 00976 } 00977 if(RotateCounter == 7 && newYaw_Moebius >10.0f && newYaw_Moebius < 180.0f){ 00978 precounter++; 00979 if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;} 00980 } 00981 00982 00983 if(RotateCounter < 6) UpdateTargetAngle_Rightloop_short(targetAngle); 00984 else UpdateTargetAngle_Leftloop_short(targetAngle); //左旋回 00985 } 00986 00987 //自動滑空 00988 void UpdateTargetAngle_Glide(float targetAngle[3]){ 00989 static int THRcount = 0; 00990 static int t_start = 0; 00991 static bool flg_tstart = false; 00992 int t_diff = 0; 00993 static int groundcount = 0; 00994 00995 targetAngle[ROLL] = g_glideloopROLL; 00996 targetAngle[PITCH] = g_glideloopPITCH; 00997 00998 autopwm[THR]=oldTHR; 00999 //シリアル通信受信の割り込みイベント登録 01000 //pc.attach(ISR_Serial_Rx, Serial::RxIrq); 01001 01002 /* 01003 //時間計測開始設定 01004 if(!flg_tstart && CheckSW_Up(Ch7)){ 01005 t_start = t.read(); 01006 flg_tstart = true; 01007 pc.printf("timer start\r\n"); 01008 }else if(!CheckSW_Up(Ch7)){ 01009 t_start = 0; 01010 flg_tstart = false; 01011 } 01012 01013 01014 //フラグが偽であれば計測は行わない 01015 if(flg_tstart){ 01016 t_diff = t.read() - t_start; 01017 //一定高度or15秒でled点灯 01018 if((groundcount>5 && g_distance>0) || t_diff > 15){ 01019 led2 = 1; 01020 //pc.printf("Call [Stop!] calling!\r\n"); 01021 } 01022 if(g_distance<180 && g_distance > 0) groundcount++; 01023 }else{ 01024 t_diff = 0; 01025 groundcount = 0; 01026 led2 = 0; 01027 } 01028 01029 if(t_diff > 17){ 01030 autopwm[THR] = SetTHRinRatio(0.5); 01031 }else{ 01032 if(g_distance<150 && g_distance>0 ){ 01033 THRcount++; 01034 if(THRcount>5){ 01035 autopwm[THR] = SetTHRinRatio(0.6); 01036 //pc.printf("throttle ON\r\n"); 01037 } 01038 }else{ 01039 autopwm[THR] = 1180; 01040 THRcount = 0; 01041 } 01042 }*/ 01043 } 01044 01045 //離陸-投下-着陸一連 01046 void Take_off_and_landing(float targetAngle[3]){ 01047 if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; 01048 01049 switch(bombing_mode){ 01050 case Takeoff: 01051 static bool flg_setFirstYaw = false; 01052 static int TakeoffCount = 0; 01053 01054 if(!flg_setFirstYaw && CheckSW_Up(Ch7)){ 01055 FirstYAW = nowAngle[YAW] + FirstYAW; 01056 flg_setFirstYaw = true; 01057 }else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){ 01058 flg_setFirstYaw = false; 01059 } 01060 01061 UpdateTargetAngle_Takeoff(targetAngle); 01062 01063 if(g_distance>150) TakeoffCount++; 01064 else TakeoffCount = 0; 01065 if(TakeoffCount>5){ 01066 led2=1; 01067 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01068 pc.printf("Now go to Approach mode!!"); 01069 bombing_mode = Approach; 01070 led2=0; 01071 } 01072 01073 break; 01074 /* 01075 case Chicken: 01076 break; 01077 */ 01078 case Transition: 01079 /* 01080 static int ApproachCount = 0; 01081 targetAngle[YAW]=180.0; 01082 int Judge = Rotate(targetAngle, g_SerialTargetYAW); 01083 01084 if(Judge==0) ApproachCount++; 01085 if(ApproachCount>5) bombing_mode = Approach; 01086 */ 01087 break; 01088 01089 case Approach: 01090 UpdateTargetAngle_Approach(targetAngle); 01091 break; 01092 01093 default: 01094 bombing_mode = Takeoff; 01095 break; 01096 } 01097 } 01098 01099 //離陸モード 01100 void UpdateTargetAngle_Takeoff(float targetAngle[3]){ 01101 static int tELE_start = 0; 01102 static bool flg_ELEup = false; 01103 int t_def = 0; 01104 if(!flg_ELEup && CheckSW_Up(Ch7)){ 01105 tELE_start = t.read_ms(); 01106 flg_ELEup = true; 01107 pc.printf("timer start\r\n"); 01108 }else if(!CheckSW_Up(Ch7)){ 01109 tELE_start = 0; 01110 flg_ELEup = false; 01111 } 01112 if(flg_ELEup){ 01113 t_def = t.read_ms() - tELE_start; 01114 01115 //1.5秒経過すればELE上げ舵へ 01116 if(t_def>500) targetAngle[PITCH]=-25.0; 01117 else{ 01118 t_def = 0; 01119 targetAngle[PITCH]=g_gostraightPITCH; 01120 } 01121 } 01122 autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 01123 targetAngle[ROLL] = 3.5; 01124 /* 01125 //pc.printf("%d \r\n",g_distance); 01126 targetAngle[ROLL] = g_gostraightROLL; 01127 if UptargetAngle[PITCH] = -10.0; 01128 autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 01129 */ 01130 } 01131 01132 //ヨーを目標値にして許容角度になるまで水平旋回 01133 char Rotate(float targetAngle[3], float TargetYAW, char mode){ 01134 float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); 01135 01136 switch(mode){ 01137 case 'G': //直進 01138 UpdateTargetAngle_GoStraight(targetAngle); 01139 if(diffYaw > LIMIT_STRAIGHT_YAW) mode = 'R'; 01140 else if(diffYaw < -LIMIT_STRAIGHT_YAW) mode = 'L'; 01141 break; 01142 01143 case 'R': //右旋回 01144 UpdateTargetAngle_Rightloop_short(targetAngle); 01145 if(diffYaw < LIMIT_LOOPSTOP_YAW && diffYaw > -LIMIT_STRAIGHT_YAW) mode = 'G'; 01146 break; 01147 01148 case 'L': //左旋回 01149 UpdateTargetAngle_Leftloop_short(targetAngle); 01150 if(diffYaw > -LIMIT_LOOPSTOP_YAW && diffYaw < LIMIT_STRAIGHT_YAW) mode = 'G'; 01151 break; 01152 } 01153 01154 return mode; 01155 01156 /* 01157 if(diffYaw > LIMIT_STRAIGHT_YAW){ 01158 UpdateTargetAngle_Rightloop_short(targetAngle); 01159 return 1; 01160 }else if(diffYaw < -LIMIT_STRAIGHT_YAW){ 01161 UpdateTargetAngle_Leftloop_short(targetAngle); 01162 return 1; 01163 }else{ 01164 UpdateTargetAngle_GoStraight(targetAngle); 01165 return 0; 01166 } 01167 */ 01168 } 01169 01170 //チキラー投下 01171 void Chicken_Drop(){ 01172 if(CheckSW_Up(Ch7)){ 01173 autopwm[DROP] = 1572; 01174 pc.printf("Bombed!\r\n"); 01175 RerurnChickenServo1.attach(&ReturnChickenServo1, 3); 01176 //operation_mode = Approach; 01177 //buzzer = 0; 01178 } 01179 } 01180 01181 void ReturnChickenServo1(){ 01182 autopwm[DROP] = 1438; 01183 pc.printf("first reverse\r\n"); 01184 RerurnChickenServo2.attach(&ReturnChickenServo2, 1); 01185 } 01186 01187 void ReturnChickenServo2(){ 01188 autopwm[DROP] = 1446; 01189 pc.printf("second reverse\r\n"); 01190 } 01191 01192 //着陸モード（PCからの指令に従う） 01193 void UpdateTargetAngle_Approach(float targetAngle[3]){ 01194 static char rotatemode = 'G'; 01195 if(output_status == Manual) rotatemode = 'G'; 01196 01197 //pc.putc(g_landingcommand); 01198 switch(g_landingcommand){ 01199 case 'R': //右旋回セヨ 01200 UpdateTargetAngle_Rightloop(targetAngle); 01201 break; 01202 01203 case 'L': //左旋回セヨ 01204 UpdateTargetAngle_Leftloop(targetAngle); 01205 break; 01206 01207 case 'G': //直進セヨ 01208 UpdateTargetAngle_GoStraight(targetAngle); 01209 break; 01210 01211 case 'Y': //指定ノヨー方向ニ移動セヨ 01212 rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode); 01213 break; 01214 01215 case 'B': //ブザーヲ鳴ラセ 01216 //buzzer = 1; 01217 break; 01218 01219 case 'D': //物資ヲ落トセ 01220 Chicken_Drop(); 01221 break; 01222 01223 case 'C': //停止セヨ 01224 rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode); 01225 autopwm[THR] = minpwm[THR]; 01226 break; 01227 } 01228 } 01229 01230 void checkHeight(float targetAngle[3]){ 01231 01232 static int targetHeight = 200; 01233 01234 if(g_distance < targetHeight + ALLOWHEIGHT){ 01235 UpdateTargetAngle_NoseUP(targetAngle); 01236 if(CheckSW_Up(Ch7)) led2 = 1; 01237 } 01238 else if(g_distance > targetHeight - ALLOWHEIGHT){ 01239 UpdateTargetAngle_NoseDOWN(targetAngle); 01240 if(CheckSW_Up(Ch7)) led2 = 1; 01241 } 01242 else led2=0; 01243 } 01244 01245 void UpdateTargetAngle_NoseUP(float targetAngle[3]){ 01246 01247 //targetAngle[PITCH] += 2.0f; 01248 //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6); 01249 autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05f); 01250 //pc.printf("nose UP"); 01251 } 01252 01253 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){ 01254 01255 //targetAngle[PITCH] -= 2.0f; 01256 autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05f); 01257 //pc.printf("nose DOWN"); 01258 } 01259 01260 //直進 01261 void UpdateTargetAngle_GoStraight(float targetAngle[3]){ 01262 01263 targetAngle[ROLL] = g_gostraightROLL; 01264 targetAngle[PITCH] = g_gostraightPITCH; 01265 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01266 01267 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01268 } 01269 01270 //右旋回 01271 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 01272 01273 targetAngle[ROLL] = g_rightloopROLL; 01274 targetAngle[PITCH] = g_rightloopPITCH ; 01275 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01276 //checkHeight(targetAngle); 01277 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01278 } 01279 01280 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回 01281 01282 targetAngle[ROLL] = g_rightloopROLLshort; 01283 targetAngle[PITCH] = g_rightloopPITCHshort; 01284 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01285 01286 01287 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01288 } 01289 01290 //左旋回 01291 void UpdateTargetAngle_Leftloop(float targetAngle[3]){ 01292 01293 targetAngle[ROLL] = g_leftloopROLL; 01294 targetAngle[PITCH] = g_leftloopPITCH; 01295 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01296 //checkHeight(targetAngle); 01297 01298 //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01299 } 01300 01301 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){ 01302 01303 targetAngle[ROLL] = g_leftloopROLLshort; 01304 targetAngle[PITCH] = g_leftloopPITCHshort; 01305 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01306 01307 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01308 } 01309 01310 //デバッグ用 01311 void DebugPrint(){ 01312 /* 01313 static int16_t deltaT = 0, t_start = 0; 01314 deltaT = t.read_u2s() - t_start; 01315 pc.printf("t:%d us, ",deltaT); 01316 pc.printf("\r\n"); 01317 t_start = t.read_us(); 01318 */ 01319 01320 //pc.printf("%d", sbus.manualpwm[4]); 01321 01322 //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); 01323 //for(uint8_t i=1; i<5; i++) pc.printf("%d ",sbus.manualpwm[i]); 01324 //pc.printf("\r\n"); 01325 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); 01326 //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ 01327 //pc.printf("%d\t",autopwm[ELE]); pc.printf("%d\t",autopwm[RUD]); 01328 //pc.printf("%d",g_distance); 01329 01330 //pc.printf("Mode: %c: ",g_buf[0]); 01331 //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); 01332 //pc.printf("%x ",sbus.failsafe_status); 01333 01334 //pc.printf("\r\n"); 01335 } 01336 01337 void ISR_Serial_Rx() 01338 { 01339 // シリアルの受信処理 01340 char data = pc.getc(); 01341 01342 if(data=='C'){ 01343 autopwm[THR]=minpwm[2]; 01344 wait(60.0); 01345 } 01346 01347 }
Generated on Wed Jul 13 2022 17:35:46 by
1.7.2