![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
自動着陸の右旋回を修正
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_24 by
Diff: main.cpp
- Revision:
- 3:206b17251f5b
- Parent:
- 2:23daa5fa28b4
- Child:
- 4:fff1165ca50c
diff -r 23daa5fa28b4 -r 206b17251f5b main.cpp --- a/main.cpp Sat Sep 08 05:32:55 2018 +0000 +++ b/main.cpp Thu Sep 13 11:34:19 2018 +0000 @@ -35,7 +35,7 @@ #define GAIN_CONTROLVALUE_TO_PWM 3.0 #define RIGHT_ROLL -12.0 -#define RIGHT_PITCH -5.0 +#define RIGHT_PITCH 5.0 #define LEFT_ROLL 12.0 #define LEFT_PITCH -5.0 #define STRAIGHT_ROLL 4.0 @@ -50,9 +50,24 @@ #define LEFT_ROLL_SHORT 12.0 #define LEFT_PITCH_SHORT -5.0 +#define rightloopRUD 1250 +#define AIL_L_correctionrightloop 0 +#define rightloopshortRUD 1250 +#define AIL_L_correctionrightloopshort 0 +#define leftloopRUD 1500 +#define AIL_L_correctionleftloop -0 +#define leftloopshortRUD 1500 +#define AIL_L_correctionleftloopshort 0 +#define glideloopRUD 1300 + + #define GLIDE_ROLL -12.0 #define GLIDE_PITCH -3.0 + +#define AIL_L_RatioRising 0.5 +#define AIL_L_RatioDescent 2 + //コンパスキャリブレーション //SkipperS2基板 /* @@ -86,7 +101,7 @@ const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]}; const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]}; const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]}; -const int16_t reverce[4] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; +const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; SBUS sbus(PA_9, PA_10); //SBUS @@ -730,7 +745,7 @@ addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ:右回転正(8月13日時点;左回転が正!) レバー:右回転正 autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite - autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[AIL_R]; + autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[ROLL]; //autopwm[THR] = oldTHR; autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); @@ -1183,15 +1198,19 @@ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } + //右旋回 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 - autopwm[RUD]=g_rightloopRUD; targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; - autopwm[THR] = oldTHR;//SetTHRinRatio(g_loopTHR); - autopwm[AIL_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*0.5;//*inverse;//g_AIL_L_Ratio_rightloop; - pc.printf("",) + autopwm[RUD]=rightloopRUD; //RUD固定 + autopwm[THR] = oldTHR; //手動スロットル記憶 + 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; + //checkHeight(targetAngle); //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } @@ -1200,8 +1219,12 @@ targetAngle[ROLL] = g_rightloopROLLshort; targetAngle[PITCH] = g_rightloopPITCHshort; - autopwm[THR] = SetTHRinRatio(g_loopTHR); - + autopwm[RUD]=rightloopshortRUD; + autopwm[THR] = oldTHR; + if(autopwm[AIL_R]>trimpwm[AIL_R]){ + autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; + } + else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); }