フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
Diff: main.cpp
- Revision:
- 39:b8d5be233b70
- Parent:
- 38:ef0f39dfc98a
- Child:
- 40:f03b62a3b495
--- a/main.cpp Tue Sep 25 09:43:16 2018 +0000 +++ b/main.cpp Tue Sep 25 10:27:21 2018 +0000 @@ -49,6 +49,9 @@ #define RIGHT_PITCH_SHORT -5.0 #define LEFT_ROLL_SHORT 12.0 #define LEFT_PITCH_SHORT -5.0 +#define RIGHTLOOPROLL_APPROACH -17.0 +#define LEFTLOOPROLL_APPROACH 16.0 + #define rightloopROLL2 -10.0 @@ -69,7 +72,10 @@ #define RIGHTLOOPSHORT_RUD 1250 #define LEFTLOOP_RUD 1500 #define LEFTLOOPSHORT_RUD 1500 -#define GLIDELOOP_RUD 1300 +#define GLIDELOOP_RUD 1300 +#define RIGHTLOOPRUD_APPROACH 1500 +#define LEFTLOOPRUD_APPROACH 1500 + #define AIL_L_CORRECTION_RIGHTLOOP 0 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0 #define AIL_L_CORRECTION_LEFTLOOP 0 @@ -241,8 +247,8 @@ int *g_rightloopRUD, int *g_rightloopshortRUD, int *g_leftloopRUD, int *g_leftloopshortRUD, int *g_glideRUD, - int *g_AIL_L_correctionrightloop,int *g_AIL_L_correctionrightloopshort, - int *g_AIL_L_correctionlefttloop,int *g_AIL_L_correctionleftloopshort + float *g_rightloopROLL_approach,float *g_leftloopROLL_approach, + int *g_rightloopRUD_approach,int *g_leftloopRUD_approach ); //switch2割り込み void ResetTrim(); @@ -321,8 +327,8 @@ &g_rightloopRUD, &g_rightloopshortRUD, &g_leftloopRUD, &g_leftloopshortRUD, &g_glideloopRUD, - &g_AIL_L_correctionrightloop,&g_AIL_L_correctionrightloopshort, - &g_AIL_L_correctionleftloop,&g_AIL_L_correctionleftloopshort + &g_rightloopROLL_approach,&g_leftloopROLL_approach, + &g_rightloopRUD_approach,&g_leftloopRUD_approach ); @@ -635,8 +641,8 @@ int *g_rightloopRUD, int *g_rightloopshortRUD, int *g_leftloopRUD, int *g_leftloopshortRUD, int *g_glideloopRUD, - int *g_AIL_L_correctionrightloop,int *g_AIL_L_correctionrightloopshort, - int *g_AIL_L_correctionleftloop,int *g_AIL_L_correctionleftloopshort + float *g_rightloopROLL_approach,float *g_leftloopROLL_approach, + int *g_rightloopRUD_approach,int *g_leftloopRUD_approach ){ pc.printf("SDsetup start.\r\n"); @@ -673,10 +679,10 @@ "LEFTLOOP_RUDDER", "LEFTLOOPSHORT_RUDDER", "GLIDELOOP_RUDDER", - "AILERON_LEFT_CORRECTION_RIGHTLOOP", - "AILERON_LEFT_CORRECTION_RIGHTLOOPSHORT", - "AILERON_LEFT_CORRECTION_LEFTLOOP", - "AILERON_LEFT_CORRECTION_LEFTLOOPSHORT" + "RIGHTLOOP_ROLL_APPROACH", + "LEFTLOOP_ROLL_APPROACH", + "RIGHTLOOP_RUDDER_APPROACH", + "LEFTLOOP_RUDDER_APPROACH" }; fp = fopen("/sd/option.txt","r"); @@ -795,20 +801,20 @@ else{ *g_glideloopRUD = GLIDELOOP_RUD; SDerrorcount++; } - if(GetParameter(fp,paramNames[28],parameter)) *g_AIL_L_correctionrightloop = atof(parameter); - else{ *g_AIL_L_correctionrightloop = AIL_L_CORRECTION_RIGHTLOOP; + if(GetParameter(fp,paramNames[28],parameter)) *g_rightloopROLL_approach = atof(parameter); + else{ *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH; SDerrorcount++; } - if(GetParameter(fp,paramNames[29],parameter)) *g_AIL_L_correctionrightloopshort = atof(parameter); - else{ *g_AIL_L_correctionrightloopshort = AIL_L_CORRECTION_RIGHTLOOPSHORT; + if(GetParameter(fp,paramNames[29],parameter)) *g_leftloopROLL_approach= atof(parameter); + else{ *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH; SDerrorcount++; } - if(GetParameter(fp,paramNames[30],parameter)) *g_AIL_L_correctionleftloop = atof(parameter); - else{ *g_AIL_L_correctionleftloop = AIL_L_CORRECTION_LEFTLOOP; + if(GetParameter(fp,paramNames[30],parameter)) *g_rightloopRUD_approach = atof(parameter); + else{ *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH; SDerrorcount++; } - if(GetParameter(fp,paramNames[31],parameter)) *g_AIL_L_correctionleftloopshort = atof(parameter); - else{ *g_AIL_L_correctionleftloopshort = AIL_L_CORRECTION_LEFTLOOPSHORT; + if(GetParameter(fp,paramNames[31],parameter)) *g_leftloopRUD= atof(parameter); + else{ *g_leftloopRUD= LEFTLOOPRUD_APPROACH; SDerrorcount++; } @@ -838,10 +844,10 @@ *g_leftloopRUD = LEFTLOOP_RUD; *g_leftloopshortRUD = LEFTLOOPSHORT_RUD; *g_glideloopRUD = GLIDELOOP_RUD; - *g_AIL_L_correctionrightloop = AIL_L_CORRECTION_RIGHTLOOP; - *g_AIL_L_correctionrightloopshort = AIL_L_CORRECTION_RIGHTLOOPSHORT; - *g_AIL_L_correctionleftloop = AIL_L_CORRECTION_LEFTLOOP; - *g_AIL_L_correctionleftloopshort = AIL_L_CORRECTION_LEFTLOOPSHORT; + *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH; + *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH; + *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH; + *g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH; SDerrorcount = -1; @@ -864,8 +870,8 @@ 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); + pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",*g_rightloopROLL_approach,*g_leftloopROLL_approach); + pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",*g_rightloopRUD_approach,*g_leftloopRUD_approach); return SDerrorcount; } @@ -1404,9 +1410,9 @@ UpdateTargetAngle_Rightloop_zero(targetAngle); } else{ - targetAngle[ROLL] = g_rightloopROLL_aproach; + targetAngle[ROLL] = g_rightloopROLL_approach; targetAngle[PITCH] = g_rightloopPITCH ; - autopwm[RUD]=g_rightloopRUD_aproach; //RUD固定 + autopwm[RUD]=g_rightloopRUD_approach; //RUD固定 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; } @@ -1422,9 +1428,9 @@ UpdateTargetAngle_Leftloop_zero(targetAngle); } else{ - targetAngle[ROLL] = g_leftloopROLL_aproach; + targetAngle[ROLL] = g_leftloopROLL_approach; targetAngle[PITCH] = g_leftloopPITCH; - autopwm[RUD]=g_leftloopRUD_aproach; + autopwm[RUD]=g_leftloopRUD_approach; 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; } @@ -1567,9 +1573,9 @@ //右旋回(着陸時スロットル0の時) void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回 autopwm[THR]=minpwm[THR]; - targetAngle[ROLL] = g_rightloopROLL_aproach; + targetAngle[ROLL] = g_rightloopROLL_approach; targetAngle[PITCH] = g_rightloopPITCH ; - autopwm[RUD]=g_rightloopRUD_aproach; //RUD固定 + autopwm[RUD]=g_rightloopRUD_approach; //RUD固定 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; } @@ -1613,9 +1619,9 @@ //左旋回(着陸時スロットル0のとき) void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){ - targetAngle[ROLL] = g_leftloopROLL_aproach; + targetAngle[ROLL] = g_leftloopROLL_approach; targetAngle[PITCH] = g_leftloopPITCH; - autopwm[RUD]=g_leftloopRUD_aproach; + autopwm[RUD]=g_leftloopRUD_approach; 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;