航空研究会
LED入れ替え
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_55
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 @@
//右旋回（着陸時スロットル０の時）
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 @@
//左旋回（着陸時スロットル０のとき）
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;