フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
Diff: main.cpp
- Revision:
- 9:f6367b7fd7be
- Parent:
- 8:66bba39b95a9
- Child:
- 10:652071c20bf6
diff -r 66bba39b95a9 -r f6367b7fd7be main.cpp --- a/main.cpp Fri Sep 14 05:56:59 2018 +0000 +++ b/main.cpp Sat Sep 15 06:14:03 2018 +0000 @@ -20,9 +20,9 @@ #define DEBUG_SEMIAUTO 0 #define DEBUG_PRINT_INLOOP 1 -#define KP_ELE 2.0 +#define KP_ELE 15.0 //2.0 #define KI_ELE 0.0 -#define KD_ELE 0.0 +#define KD_ELE 0.0 //0/0 #define KP_RUD 3.0 #define KI_RUD 0.0 #define KD_RUD 0.0 @@ -35,7 +35,7 @@ #define GAIN_CONTROLVALUE_TO_PWM 3.0 #define RIGHT_ROLL -12.0 -#define RIGHT_PITCH 5.0 +#define RIGHT_PITCH -10.0 //5.0 #define LEFT_ROLL 12.0 #define LEFT_PITCH -5.0 #define STRAIGHT_ROLL 4.0 @@ -50,8 +50,9 @@ #define LEFT_ROLL_SHORT 12.0 #define LEFT_PITCH_SHORT -5.0 -#define rightloopRUD 1250 -#define AIL_L_correctionrightloop -200 +#define rightloopRUD 1300 //1250 +#define AIL_R_correctionrightloop 0 +#define AIL_L_correctionrightloop -400 #define rightloopshortRUD 1250 #define AIL_L_correctionrightloopshort 0 #define leftloopRUD 1500 @@ -344,7 +345,7 @@ //Rotate(targetAngle, 30.0); CalculateControlValue(targetAngle, controlValue); UpdateAutoPWM(controlValue); - wait_ms(30); + wait_ms(25); #if DEBUG_PRINT_INLOOP DebugPrint(); #endif @@ -1215,12 +1216,13 @@ targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; autopwm[RUD]=rightloopRUD; //RUD固定 - autopwm[THR] = oldTHR; //手動スロットル記憶 + autopwm[THR] = SetTHRinRatio(0.5); //手動スロットル記憶 if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; - + autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; + //checkHeight(targetAngle); //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); }