航空研究会
9/13 updated
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_4_Approach
by 
航空研究会
Diff: main.cpp
- Revision:
- 5:eca3adcd8f9c
- Parent:
- 4:b66ab0bee119
--- a/main.cpp Thu Sep 13 06:16:12 2018 +0000
+++ b/main.cpp Thu Sep 13 06:49:58 2018 +0000
@@ -42,15 +42,15 @@
#define TAKEOFF_THR 0.8
#define LOOP_THR 0.6
-#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 RIGHTLOOP_RUD 1250
+#define RIGHTLOOPSHORT_RUD 1250
+#define LEFTLOOP_RUD 1500
+#define LEFTLOOPSHORT_RUD 1500
+#define GLIDELOOP_RUD 1300
+#define AIL_L_CORRECTION_RIGHTLOOP 0
+#define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
+#define AIL_L_CORRECTION_LEFTLOOP 0
+#define AIL_L_CORRECTION_LEFTLOOPSHORT 0
#define AIL_L_RatioRising 0.5
#define AIL_L_RatioDescent 2
@@ -209,7 +209,14 @@
float *g_takeoffTHR, float *g_loopTHR,
float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
- float *g_glideloopROLL, float *g_glideloopPITCH);
+ float *g_glideloopROLL, float *g_glideloopPITCH,
+ float *g_kpAIL, float *g_kiAIL, float *g_kdAIL,
+ 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
+ );
//switch2割り込み
void ResetTrim();
@@ -278,8 +285,14 @@
&g_takeoffTHR, &g_loopTHR,
&g_rightloopROLLshort, &g_rightloopPITCHshort,
&g_leftloopROLLshort, &g_leftloopPITCHshort,
- &g_glideloopROLL, &g_glideloopPITCH);
-
+ &g_glideloopROLL, &g_glideloopPITCH,
+ &g_kpAIL, &g_kiAIL,&g_kdAIL,
+ &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
+ );
Init_PWM();
Init_servo();
@@ -570,13 +583,19 @@
float *g_takeoffTHR, float *g_loopTHR,
float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
- float *g_glideloopROLL, float *g_glideloopPITCH
+ float *g_glideloopROLL, float *g_glideloopPITCH,
+ float *g_kpAIL, float *g_kiAIL, float *g_kdAIL,
+ 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
){
pc.printf("SDsetup start.\r\n");
FILE *fp;
- char parameter[30]; //文字列渡す用の配列
+ char parameter[40]; //文字列渡す用の配列
int SDerrorcount = 0; //取得できなかった数を返す
const char *paramNames[] = {
"KP_ELEVATOR",
@@ -598,7 +617,19 @@
"LEFTLOOP_ROLL_SHORT",
"LEFTLOOP_PITCH_SHORT",
"AUTOGLIDE_ROLL",
- "AUTOGLIDE PITCH"
+ "AUTOGLIDE PITCH",
+ "KP_AILERON",
+ "KI_AILERON",
+ "KD_AILERON",
+ "RIGHTLOOP_RUDDER",
+ "RIGHTLOOPSHORT_RUDDER",
+ "LEFTLOOP_RUDDER",
+ "LEFTLOOPSHORT_RUDDER",
+ "GLIDELOOP_RUDDER",
+ "AILERON_LEFT_CORRECTION_RIGHTLOOP",
+ "AILERON_LEFT_CORRECTION_RIGHTLOOPSHORT",
+ "AILERON_LEFT_CORRECTION_LEFTLOOP",
+ "AILERON_LEFT_CORRECTION_LEFTLOOPSHORT"
};
fp = fopen("/sd/option.txt","r");
@@ -685,6 +716,54 @@
else{ *g_glideloopPITCH = GLIDE_PITCH;
SDerrorcount++;
}
+ if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter);
+ else{ *g_kpAIL = KP_AIL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter);
+ else{ *g_kiAIL = KI_AIL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter);
+ else{ *g_kdAIL = KP_AIL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter);
+ else{ *g_rightloopRUD = RIGHTLOOP_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter);
+ else{ *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter);
+ else{ *g_leftloopshortRUD = LEFTLOOP_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter);
+ else{ *g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter);
+ 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;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[29],parameter)) *g_AIL_L_correctionrightloopshort = atof(parameter);
+ else{ *g_AIL_L_correctionrightloopshort = AIL_L_CORRECTION_RIGHTLOOPSHORT;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[30],parameter)) *g_AIL_L_correctionleftloop = atof(parameter);
+ else{ *g_AIL_L_correctionleftloop = AIL_L_CORRECTION_LEFTLOOP;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[31],parameter)) *g_AIL_L_correctionleftloopshort = atof(parameter);
+ else{ *g_AIL_L_correctionleftloopshort = AIL_L_CORRECTION_LEFTLOOPSHORT;
+ SDerrorcount++;
+ }
fclose(fp);
}else{ //ファイルがなかったら
@@ -703,6 +782,26 @@
*g_gostraightPITCH = STRAIGHT_PITCH;
*g_takeoffTHR = TAKEOFF_THR;
*g_loopTHR = LOOP_THR;
+ *g_rightloopROLLshort = RIGHT_ROLL_SHORT;
+ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT;
+ *g_leftloopROLLshort = LEFT_ROLL_SHORT;
+ *g_leftloopPITCHshort = LEFT_PITCH_SHORT;
+ *g_glideloopROLL = GLIDE_ROLL;
+ *g_glideloopPITCH = GLIDE_PITCH;
+ *g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく！！
+ *g_kiAIL = KI_AIL;
+ *g_kdAIL = KD_AIL;
+ *g_rightloopRUD = RIGHTLOOP_RUD;
+ *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
+ *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;
+
+
SDerrorcount = -1;
}
pc.printf("SDsetup finished.\r\n");
@@ -721,7 +820,8 @@
pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH);
return SDerrorcount;
-}
+}
+
void CalculateControlValue(float targetAngle[3], float controlValue[3]){
static int t_last;
@@ -981,7 +1081,7 @@
targetAngle[ROLL] = g_glideloopROLL;
targetAngle[PITCH] = g_glideloopPITCH;
- autopwm[RUD]=glideloopRUD;
+ autopwm[RUD]=g_glideloopRUD;
//autopwm[THR]=oldTHR;
//シリアル通信受信の割り込みイベント登録
@@ -1219,55 +1319,60 @@
autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05);
//pc.printf("nose DOWN");
}
-
//直進
void UpdateTargetAngle_GoStraight(float targetAngle[3]){
targetAngle[ROLL] = g_gostraightROLL;
targetAngle[PITCH] = g_gostraightPITCH;
autopwm[THR] = SetTHRinRatio(g_loopTHR);
+ if(autopwm[AIL_R]>trimpwm[AIL_R]){
+ autopwm[AIL_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+(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]);
}
-
//直進(着陸時throttle０の時）
void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]){
targetAngle[ROLL] = g_gostraightROLL;
targetAngle[PITCH] = g_gostraightPITCH;
autopwm[THR] = minpwm[THR];
+if(autopwm[AIL_R]>trimpwm[AIL_R]){
+ autopwm[AIL_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+(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 UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
+
+ targetAngle[ROLL] = g_rightloopROLL;
+ targetAngle[PITCH] = g_rightloopPITCH ;
+ autopwm[RUD]=g_rightloopRUD; //RUD固定
+ autopwm[THR] = oldTHR; //手動スロットル記憶
+ if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節
+ autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+g_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_zero(float targetAngle[3]){ //右旋回
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;
+ autopwm[AIL_L]=trimpwm[AIL_L]+g_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(float targetAngle[3]){ //右旋回
-
- targetAngle[ROLL] = g_rightloopROLL;
- targetAngle[PITCH] = g_rightloopPITCH ;
- autopwm[RUD]=rightloopRUD;
- autopwm[THR] = oldTHR;//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;
+ else autopwm[AIL_L]=trimpwm[AIL_L]+g_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]);
@@ -1277,12 +1382,12 @@
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;
+ autopwm[AIL_L]=trimpwm[AIL_L]+g_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;
+ else autopwm[AIL_L]=trimpwm[AIL_L]+g_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]);
}
@@ -1292,13 +1397,12 @@
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;
+ autopwm[AIL_L]=trimpwm[AIL_L]+g_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;
+ else autopwm[AIL_L]=trimpwm[AIL_L]+g_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]);
@@ -1309,12 +1413,12 @@
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;
+ autopwm[AIL_L]=trimpwm[AIL_L]+g_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;
+ else autopwm[AIL_L]=trimpwm[AIL_L]+g_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);
@@ -1325,15 +1429,16 @@
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;
+ autopwm[AIL_L]=trimpwm[AIL_L]+g_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;
+ else autopwm[AIL_L]=trimpwm[AIL_L]+g_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()
{
// シリアルの受信処理