フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
Diff: main.cpp
- Revision:
- 34:5719e6977ec7
- Parent:
- 33:0f7a35d55316
- Child:
- 35:25e1afadd455
diff -r 0f7a35d55316 -r 5719e6977ec7 main.cpp --- a/main.cpp Sun Sep 23 18:17:50 2018 +0000 +++ b/main.cpp Mon Sep 24 04:24:51 2018 +0000 @@ -1268,7 +1268,8 @@ NVIC_EnableIRQ(EXTI9_5_IRQn); if(TakeoffCount>5){ autopwm[THR] = 1180+320*2*0.5; - pc.printf("Now go to Approach mode!!"); + autopwm[ELE] = 1200; + //pc.printf("Now go to Approach mode!!"); bombing_mode = Approach; } break; @@ -1302,11 +1303,33 @@ //離陸モード void UpdateTargetAngle_Takeoff(float targetAngle[3]){ //pc.printf("%d \r\n",g_distance); + static int tELE_start = 0; + static bool flg_ELEup = false; + int t_def = 0; + if(!flg_ELEup && CheckSW_Up(Ch7)){ + tELE_start = t.read_ms(); + flg_ELEup = true; + pc.printf("timer start\r\n"); + }else if(!CheckSW_Up(Ch7)){ + tELE_start = 0; + flg_ELEup = false; + } + if(flg_ELEup){ + t_def = t.read_ms() - tELE_start; + + //1.5秒経過すればELE上げ舵へ + if(t_def>500) targetAngle[PITCH]=-30.0; + else{ + t_def = 0; + targetAngle[PITCH]=g_gostraightPITCH; + } + } targetAngle[ROLL] = g_gostraightROLL; - targetAngle[PITCH] = g_loopTHR; + //targetAngle[PITCH] = g_loopTHR; autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 } + //ヨーを目標値にして許容角度になるまで水平旋回 int Rotate(float targetAngle[3], float TargetYAW){ float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);