Revision 5:eca3adcd8f9c, committed 2018-09-13

Comitter:

HARUKIDELTA

Date:

Thu Sep 13 06:49:58 2018 +0000

Parent:

4:b66ab0bee119

Commit message:

SD setting

Changed in this revision

diff -r b66ab0bee119 -r eca3adcd8f9c config/falfalla.h
--- a/config/falfalla.h	Thu Sep 13 06:16:12 2018 +0000
+++ b/config/falfalla.h	Thu Sep 13 06:49:58 2018 +0000
@@ -31,6 +31,17 @@
 static float g_glideloopROLL = -5.0;      //rewrite
 static float g_glideloopPITCH = 0.0;
 
+static int g_rightloopRUD = 1500;
+static int g_rightloopshortRUD = 1500;
+static int g_leftloopRUD = 1500;
+static int g_leftloopshortRUD = 1500;
+static int g_glideloopRUD = 1500;
+
+static int g_AIL_L_correctionrightloop = 0;
+static int g_AIL_L_correctionrightloopshort = 0;
+static int g_AIL_L_correctionleftloop = 0;
+static int g_AIL_L_correctionleftloopshort = 0;
+
 #if NUMBER_FALFALLA == 1    //1号機
 
 const int16_t Trim_Falfalla[4] = {-155,120,0,-240};//{0,14,-100,-10};

diff -r b66ab0bee119 -r eca3adcd8f9c main.cpp
--- 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()
 {
     // シリアルの受信処理