![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_12 by
Diff: main.cpp
- Revision:
- 4:fff1165ca50c
- Parent:
- 3:206b17251f5b
- Child:
- 5:9efd35c9bb2e
--- a/main.cpp Thu Sep 13 11:34:19 2018 +0000 +++ b/main.cpp Thu Sep 13 12:06:21 2018 +0000 @@ -1215,6 +1215,22 @@ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } +//右旋回(着陸時スロットル0の時) +void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回 + + targetAngle[ROLL] = g_rightloopROLL; + targetAngle[PITCH] = g_rightloopPITCH ; + autopwm[RUD]=rightloopRUD; + autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR); + 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]); +} + void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回 targetAngle[ROLL] = g_rightloopROLLshort; @@ -1234,7 +1250,29 @@ 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; + //checkHeight(targetAngle); + + //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); +} + +//左旋回(着陸時スロットル0のとき) +void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){ + + targetAngle[ROLL] = g_leftloopROLL; + targetAngle[PITCH] = g_leftloopPITCH; + autopwm[RUD]=leftloopRUD; + autopwm[THR] = minpwm[THR]; + 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]); @@ -1244,8 +1282,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()