フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
Diff: main.cpp
- Revision:
- 16:7fca5b938da6
- Parent:
- 15:43427b0241d9
- Child:
- 17:55249ea37dff
diff -r 43427b0241d9 -r 7fca5b938da6 main.cpp --- a/main.cpp Mon Sep 17 06:40:41 2018 +0000 +++ b/main.cpp Mon Sep 17 08:58:08 2018 +0000 @@ -834,7 +834,12 @@ pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort); pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort); pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH); - + pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",*g_kpAIL,*g_kiAIL,*g_kdAIL); + pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",*g_rightloopRUD,*g_rightloopshortRUD); + pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",*g_leftloopRUD,*g_leftloopshortRUD); + pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD); + pc.printf("AIL_L_CORRECTION_RIGHTLOOP = %d, AIL_L_CORRECTION_RIGHTLOOPSHORT = %d\r\n",*g_AIL_L_correctionrightloop,*g_AIL_L_correctionrightloopshort); + pc.printf("AIL_L_CORRECTION_LEFTLOOP = %d, AIL_L_CORRECTION_LEFTLOOPSHORT = %d\r\n",*g_AIL_L_correctionleftloop,*g_AIL_L_correctionleftloopshort); return SDerrorcount; } @@ -1099,6 +1104,7 @@ targetAngle[ROLL] = g_glideloopROLL; targetAngle[PITCH] = g_glideloopPITCH; + autopwm[RUD]=g_glideloopRUD; // autopwm[THR]=oldTHR; @@ -1363,7 +1369,7 @@ targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; - autopwm[RUD]=rightloopRUD; //RUD固定 + autopwm[RUD]=g_rightloopRUD; //RUD固定 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; @@ -1380,7 +1386,7 @@ targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; - autopwm[RUD]=rightloopRUD; + autopwm[RUD]=g_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; @@ -1395,7 +1401,7 @@ targetAngle[ROLL] = g_rightloopROLLshort; targetAngle[PITCH] = g_rightloopPITCHshort; - autopwm[RUD]=rightloopshortRUD; + autopwm[RUD]=g_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; @@ -1410,7 +1416,7 @@ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; - autopwm[RUD]=leftloopRUD; + autopwm[RUD]=g_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; @@ -1426,7 +1432,7 @@ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; - autopwm[RUD]=leftloopRUD; + autopwm[RUD]=g_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; @@ -1442,7 +1448,7 @@ targetAngle[ROLL] = g_leftloopROLLshort; targetAngle[PITCH] = g_leftloopPITCHshort; - autopwm[RUD]=leftloopRUD; + autopwm[RUD]=g_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;