![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_4 by
Diff: main.cpp
- Revision:
- 3:954228fd64b3
- Parent:
- 2:23daa5fa28b4
- Child:
- 4:b66ab0bee119
diff -r 23daa5fa28b4 -r 954228fd64b3 main.cpp --- a/main.cpp Sat Sep 08 05:32:55 2018 +0000 +++ b/main.cpp Wed Sep 12 05:13:58 2018 +0000 @@ -26,11 +26,10 @@ #define KP_RUD 3.0 #define KI_RUD 0.0 #define KD_RUD 0.0 -#define KP_AIL 0.1 +#define KP_AIL 2.0 #define KI_AIL 0.2 #define KD_AIL 0.2 -//#define g_AIL_L_Ratio_rightloop 0.5 #define GAIN_CONTROLVALUE_TO_PWM 3.0 @@ -43,7 +42,18 @@ #define TAKEOFF_THR 0.8 #define LOOP_THR 0.6 -#define g_rightloopRUD 1500 +#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 AIL_L_RatioRising 0.5 +#define AIL_L_RatioDescent 2 #define RIGHT_ROLL_SHORT -12.0 #define RIGHT_PITCH_SHORT -5.0 @@ -86,7 +96,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 @@ -126,12 +136,11 @@ OperationMode operation_mode = StartUp; BombingMode bombing_mode = Takeoff; static int16_t autopwm[8] = {1500,1500,1180,1500,1392,1500}; -static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1500}; +static int16_t trimpwm[6] = {1428,1500,1180,1500,1392,1629}; int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820}; int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180}; int16_t oldTHR = 1000; -int16_t g_AIL_L_Ratio_rightloop = 0.5; static float nowAngle[3] = {0,0,0}; @@ -722,6 +731,9 @@ //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御 controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); + + //pc.printf("%f,%f",controlValue[ROLL],controlValue[PITCH]); + } void UpdateAutoPWM(float controlValue[3]){ @@ -730,7 +742,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]); @@ -776,6 +788,7 @@ for(uint8_t i=0;i<6;i++){ pwm[i] = sbus.manualpwm[i]; } + ///pc.printf("%d ,\r\n",pwm[AIL_R]);//L /*pc.printf("%d ,",pwm[0]);//R pc.printf("%d ,\r\n",pwm[5]);//L*/ oldTHR = sbus.manualpwm[THR]; @@ -789,8 +802,9 @@ pwm[RUD] = autopwm[RUD]; pwm[DROP] = autopwm[DROP]; pwm[AIL_L] = autopwm[AIL_L]; + //pc.printf("%d ,",pwm[ELE]);//ELE pc.printf("%d ,",pwm[AIL_R]);//R - pc.printf("%d ,\r\n",pwm[AIL_L]);//L + pc.printf("%d ,\r\n",pwm[AIL_R]);//L //pc.printf("update_auto\r\n"); break; @@ -923,6 +937,8 @@ static bool flg_setInStartAuto = false; static float FirstYAW_Moebius = 0.0; float newYaw_Moebius; + + if(!flg_setInStartAuto && CheckSW_Up(Ch7)){ FirstYAW_Moebius = nowAngle[YAW]; @@ -933,12 +949,14 @@ led2 = 0; } + autopwm[THR]=oldTHR; + newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n");} if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n");} - if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius < 10.0f) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");} - if(RotateCounter == 3 && newYaw_Moebius >20.0 && newYaw_Moebius < 180.0f) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");} + if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius < -10.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");} + if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");} if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle); @@ -956,12 +974,13 @@ targetAngle[ROLL] = g_glideloopROLL; targetAngle[PITCH] = g_glideloopPITCH; + autopwm[RUD]=glideloopRUD; - autopwm[THR]=oldTHR; + //autopwm[THR]=oldTHR; //シリアル通信受信の割り込みイベント登録 - pc.attach(ISR_Serial_Rx, Serial::RxIrq); + //pc.attach(ISR_Serial_Rx, Serial::RxIrq); -/* + //時間計測開始設定 if(!flg_tstart && CheckSW_Up(Ch7)){ t_start = t.read(); @@ -979,7 +998,7 @@ //一定高度or15秒でled点灯 if((groundcount>5 && g_distance>0) || t_diff > 15){ led2 = 1; - //pc.printf("Call [Stop!] calling!\r\n"); + pc.printf("Call [Stop!] calling!\r\n"); } if(g_distance<180 && g_distance > 0) groundcount++; }else{ @@ -995,13 +1014,13 @@ THRcount++; if(THRcount>5){ autopwm[THR] = SetTHRinRatio(0.6); - //pc.printf("throttle ON\r\n"); + pc.printf("throttle ON\r\n"); } }else{ autopwm[THR] = 1180; THRcount = 0; } - }*/ + } } //離陸-投下-着陸一連 void Take_off_and_landing(float targetAngle[3]){ @@ -1186,12 +1205,15 @@ //右旋回 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 - autopwm[RUD]=g_rightloopRUD; targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; + autopwm[RUD]=rightloopRUD; 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("",) + 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 +1222,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]); } @@ -1211,7 +1237,13 @@ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; - autopwm[THR] = SetTHRinRatio(g_loopTHR); + autopwm[RUD]=leftloopRUD; + autopwm[THR] = oldTHR; + if(autopwm[AIL_R]>trimpwm[AIL_R]){ + autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; + } + else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; + //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop; //checkHeight(targetAngle); //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); @@ -1221,8 +1253,13 @@ targetAngle[ROLL] = g_leftloopROLLshort; targetAngle[PITCH] = g_leftloopPITCHshort; - autopwm[THR] = SetTHRinRatio(g_loopTHR); - + autopwm[RUD]=leftloopRUD; + autopwm[THR] = oldTHR; + if(autopwm[AIL_R]>trimpwm[AIL_R]){ + autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; + } + else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(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]); } void ISR_Serial_Rx() @@ -1253,8 +1290,8 @@ //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ //pc.printf("%d\t",autopwm[ELE]); pc.printf("%d\t",autopwm[RUD]); //pc.printf("%d",g_distance); - //pc.printf("Mode: %c: ",g_buf[0]); //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); pc.printf("\r\n"); + //pc.printf("%d",usensor.get_dist_cm() } \ No newline at end of file